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Abstract 

This  research  develops  and  tests  a  precision  navigation  algorithm  fusing  optical 
and  inertial  measurements  of  unknown  objects  at  unknown  locations.  It  provides  an 
alternative  to  the  Global  Positioning  System  (GPS)  as  a  precision  navigation  source, 
enabling  passive  and  low-cost  navigation  in  situations  where  GPS  is  denied/unavailable. 

This  paper  describes  two  new  contributions.  Eirst,  a  rigorous  study  of  the 
fundamental  nature  of  optic al/inertial  navigation  is  accomplished  by  examining  the 
observability  grammian  of  the  underlying  measurement  equations.  This  analysis  yields  a 
set  of  design  principles  guiding  the  development  of  optical/inertial  navigation  algorithms. 

The  second  contribution  of  this  research  is  the  development  and  flight  test  of  an 
optical-inertial  navigation  system  using  low-cost  and  passive  sensors  (including  an 
inexpensive  commercial-grade  inertial  sensor,  which  is  unsuitable  for  navigation  by 
itself).  This  prototype  system  was  built  and  flight  tested  at  the  El.S.  Air  Eorce  Test  Pilot 
School.  The  algorithm  that  was  implemented  leveraged  the  design  principles  described 
above,  and  used  images  from  a  single  camera.  It  was  shown  (and  explained  by  the 
observability  analysis)  that  the  system  gained  significant  performance  by  aiding  it  with  a 
barometric  altimeter  and  magnetic  compass,  and  by  using  a  digital  terrain  database 
(DTED).  The  (still)  low-cost  and  passive  system  demonstrated  performance  comparable 
to  high  quality  navigation-grade  inertial  navigation  systems,  which  cost  an  order  of 
magnitude  more  than  this  optical-inertial  prototype.  The  resultant  performance  of  the 
system  tested  provides  a  robust  and  practical  navigation  solution  for  Air  Eorce  aircraft. 
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DEVELOPMENT  AND  FLIGHT  TEST  OF  A  ROBUST 
OPTICAL-INERTIAL  NAVIGATION  SYSTEM  USING 

LOW-COST  SENSORS 

I.  Introduction 

This  thesis  details  the  development  and  experimental  testing  of  an  airborne 
navigation  system  fusing  optical  and  inertial  sensors.  The  specific  focus  is  on  military 
utility  with  representative  flight  environments  and  equipment.  This  work  is  a  follow-on 
to  another  research  effort  at  the  Air  Force  Institute  of  Technology  (AFIT),  employing  a 
similar  navigation  strategy  [27].  Optical  inertial  navigation  is  part  of  a  concerted  effort 
by  the  Advanced  Navigation  Technology  laboratory  to  develop  precision  navigation 
technology  in  the  absence  of  the  Global  Positioning  System  (GPS),  since  GPS  can  be 
jammed  by  a  potential  adversary,  even  one  without  much  sophistication. 

1.1  Research  Motivation 

This  research  will  attempt  to  characterize  inertial  navigation  aiding  with  the  use  of 
video  feature  tracking  and  a  low-cost  micro-machined  electromechanical  system 
(MEMS)  based  inertial  navigation  system  (INS).  This  characterization  will  be  used  to 
design  a  practical  algorithm  that  provides  robust  precision  navigation  in  dynamic 
environments  for  military  application. 

Simulation  will  be  used  to  draw  conclusions  and  enhance  design.  Finally,  the 
theory  and  algorithm  will  be  evaluated  with  experimental  data  collected  as  a  flight  test 
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program  at  the  United  States  Air  Foree  (USAF)  Test  Pilot  School  (TPS),  Edwards  Air 
Force  Base  (AFB),  CA. 

The  United  States  military  holds  a  significant  advantage  on  the  battlefield,  in  part 
due  to  precision  navigation  technologies.  Precision  navigation  gives  commanders  precise 
and  accurate  information  on  the  battlefield,  and  allows  efficient  and  effective  application 
of  force,  minimizing  collateral  damage.  This  force  multiplying  competency  has  been 
increasingly  dependent  on  a  single  technology,  exposing  a  potential  Achilles  heel.  The 
military’s  dependence  on  the  Global  Positioning  System  (GPS)  for  precision  navigation 
demands  exploration  into  alternatives.  Specifically,  it  demands  alternatives  that  will 
marginalize  the  impact  of  GPS,  and  expand  the  environment  in  which  a  precision 
navigation  advantage  can  be  maintained. 

1.1.1  Importance  of  Precision  Navigation.  Navigation  is  a  critical  war  fighting 
competency,  and  its  importance  only  increases  as  technology  advances.  History  provides 
many  examples  of  the  importance  of  navigation  and  target  location;  victory  favors  the 
side  that  can  determine  these  most  accurately.  USAF  tactics  are  becoming  more  and 
more  dependent  on  GPS  quality  precision  navigation,  creating  a  need  to  protect  this 
capability.  As  navigation  technology  advances,  the  United  States  must  continually  re¬ 
evaluate  the  weaknesses  that  dependence  on  this  new  technology  exposes.  The  ability  to 
navigate,  while  denying  the  enemy  the  same,  is  critical  to  national  defense.  In  the  past, 
inertial  navigation  dominated  in  aviation,  but  has  lost  favor  to  GPS  in  recent  years. 

1.1.2  What  is  Inertial  Navigation?  Inertial  navigation  is  the  art  of  determining 
one’s  position,  and  attitude,  through  the  use  of  inertial  sensors.  Inertial  Navigation 


2 


Systems  (INSs)  use  aeeelerometers  and  gyroseopes  to  determine  a  body’s  aeeeleration 
and  angular  rotation  rate.  These  measurements  are  integrated  to  determine  position, 
veloeity,  and  attitude.  The  passive  nature  of  these  sensors  makes  them  extremely 
resistant  to  external  denial  teehniques  such  as  jamming — a  very  attractive  property  for 
military  applications. 

1.1.3  Where  is  Inertial  Navigation  Used?  Inertial  Navigation  Systems  are 
critical  components  onboard  every  United  States  Air  Force  (USAF)  combatant  aircraft, 
and  many  of  its  weapons  systems.  Promise  of  high  precision  navigation  has  led  to  many 
more  uses  of  INS  in  recent  years.  Weapons  that  used  to  be  guided  by  gravity  alone  are 
now  embedded  with  inertial  guidance  systems  (WCMD,  GBU-24,  GBU-15,  AGM-130 
[35]).  More  modern  systems  blend  INS  and  GPS.  The  Joint  Direct  Attack  Munition 
(JDAM),  favored  by  theatre  commanders,  is  a  prime  example  [30].  Unmanned  Aerial 
Vehicles  (UAVs)  require  an  INS  to  navigate  and  control  flight  path.  Individual  soldiers 
are  being  equipped  with  navigation  systems  to  help  prevent  fratricide  and  increase 
combat  effectiveness.  INSs  can  play  a  critical  role  in  that  area  as  well. 

1.1.4  Problems  with  Inertial  Navigation  Systems.  In  order  to  determine 
position  and  attitude,  accelerations  and  angular  rates  must  be  integrated.  Current  inertial 
sensors  are  very  precise,  but  not  perfect.  Unfortunately,  the  integration  of  small  inertial 
sensor  errors  causes  the  navigation  precision  to  decay  over  time.  This  property  of  an  INS 
requires  external  aiding  to  resolve.  Additionally,  the  high  quality  sensors  needed  to 
achieve  a  high  level  of  precision  are  very  expensive.  This  poses  an  economics  problem 


3 


when  attempting  to  outfit  a  single  soldier,  inexpensive  UAV,  or  disposable  weapon  with 
a  preeise  navigation  system. 

1.1.5  Aiding  a  Drifting  Inertial  Navigation  System.  Due  to  INS  integration 
errors,  INS  systems  beeome  more  and  more  unreliable  as  time  goes  on.  This  requires 
external  aiding  in  order  to  bound  error  growth.  The  errors  eannot  be  eliminated,  but  they 
ean  be  kept  from  diverging  completely.  The  concept  of  aiding  entails  taking  a 
measurement  of  the  outside  world,  and  comparing  it  to  what  the  INS  predicts  the 
measurement  to  be.  Through  stochastic  modeling  and  estimation,  the  INS  can  determine 
a  best  estimate  of  the  error  in  its  navigation  solution,  and  then  remove  it. 

1.1.6  Inertial  Navigation  Aiding.  Typically,  active  sensors,  such  as  Doppler 
radar,  synthetic  aperture  radar  maps  and  lasers,  have  been  used  to  accomplish  INS  aiding 
[25].  These  active  sensors  are  subject  to  denial  techniques  and  can  alert  the  enemy  to 
one’s  position.  For  this  reason,  active  sensors  are  undesirable  for  low  observable  aircraft 
or  for  those  entering  environments  hostile  to  such  sensors.  A  passive  sensor  is  one  that 
can  measure  the  outside  world  without  emitting  any  radiation  itself.  Passive  external 
updates  maintain  the  stealth  characteristics  desirable  on  many  platforms,  and  are  much 
more  resistant  to  denial.  Barometric  altimeters  and  optical/infra-red  cameras  are  passive 
sensors.  Beacon-based  navigation  systems,  such  as  Tactical  Air  Navigation  (TACAN) 
and  the  Global  Positioning  System,  are  also  passive,  since  the  transmitted  signal  does  not 
originate  from  the  aircraft,  but  they  are  susceptible  to  denial.  Aiding  an  INS  with  video 
imagery  provides  a  passive,  jam-resistant  solution  to  INS  aiding.  Below  is  a  list  of 
current  INS  aiding  devices  fielded  in  the  USAF  fieet: 
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•  Passive 


■  Barometric  Altimeter 

■  Forward  Looking  Infrared  (FLIR) 

■  Visual  over- fly  updates  by  the  aircrew 

■  Star  Trackers 

•  Beacon  Based 

■  Global  Positioning  System  (GPS) 

■  Tactical  Air  Navigation  (TACAN) 

■  Long-Range  Navigation  (LORAN) 

•  Active 

■  Radar  Altimeter 

■  Synthetic  Aperture  Radar 

■  Laser/Radar  Ranging 

■  Doppler  Radar 


1.1.7  Global  Positioning  System  as  an  Aiding  Source.  The  advent  of  the  GPS 
has  provided  an  inexpensive,  yet  highly  beneficial  method  of  aiding  existing  INS 
platforms  [1][7].  It  maintains  many  of  the  passive  aiding  benefits  because  the  aircraft 
need  not  emit  any  radiation;  the  GPS  satellites  transmit  all  the  signals.  Unfortunately,  the 
signal  is  quite  susceptible  to  denial.  In  recent  years,  GPS  has  become  the  preferred 
method  of  aiding  INS  onboard  USAF  tactical  aircraft  and  weapon  systems.  In  an  effort 
to  standardize  aviation  navigation  technology  across  the  three  services,  the  H-764G 
Embedded  GPS/Inertial  Navigation  System  (EGI)  was  chosen  to  outfit  US  military 
aircraft  [30].  GPS  provides  a  highly  precise  position  update  to  the  INS  at  a  1-2  Hz  rate. 
This  type  of  position  update  benefits  the  position  solution  of  the  INS  as  well  as  the 
attitude  and  velocity.  EGI  systems  provide  a  very  reliable  and  precise  navigation 
solution,  but  can  prove  to  be  a  liability  if  GPS  is  denied  in  a  particular  theatre.  Many 
cornerstone  systems  that  rely  on  precise  relative  navigation  (JDAM  and  EINK16  for 
example)  would  be  rendered  less  effective.  Certain  environments  deny  GPS  by  their  very 
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geometry;  buildings  and  tree  cover  block  the  signal.  An  aided  INS  could  fdl  the  need 
where  GPS  falls  short.  Reliance  on  GPS  as  the  sole  method  of  INS  update,  threatens  the 
ability  to  navigate  in  hostile  environments. 

1.1.8  Using  Imagery  to  Aid  Inertial  Navigation.  The  concept  of  aiding  an  INS 
with  imagery  is  quite  natural.  Our  own  brains  do  it  every  day.  The  inner  ear  contains 
semicircular  canals  that  act  like  rate  gyros,  detecting  rotation.  The  'seat  of  the  pants' 
sensation  allows  humans  to  detect  accelerations.  Granted,  this  INS  is  very  poor.  One 
needs  only  to  close  his  (or  her)  eyes  for  a  short  period  while  riding  in  a  car  before  he  or 
she  is  unsure  of  where  he  is  or  which  way  hi  is  facing.  If  that  person  opens  his  eyes,  he 
can  blend  the  images  he  sees  with  the  sensations  he  feels  to  keep  track  of  where  he  is 
going  and  which  way  he  is  facing.  Why  the  need  for  the  inertial  sensors,  though? 
Assume  that  same  person  was  sitting  in  a  bus,  looking  out  the  window  at  the  side  of 
another  bus  parked  next  to  him.  If  one  bus  started  to  move,  the  person  couldn’t  be  sure 
if  their  bus  was  moving  forward  or  the  other  bus  backward.  The  inertial  sensors  in  the 
body  resolve  that  ambiguity.  If  the  person  saw  the  other  bus  move  relative  to  himself,  but 
felt  no  acceleration,  he  knows  with  some  certainty,  what  is  happening. 

The  previous  example  was  overly  simplified,  but  the  extension  into  six  degree  of 
freedom  motion  is  where  image  aided  INS  lives.  An  aircraft  moving  along  with  an  INS 
will  have  some  idea  of  where  it  is  going,  and  where  it  is  facing.  A  camera  allows  the 
aircraft  to  track  stationary  landmarks  on  the  ground.  The  aircraft  knows  the  landmarks 
are  stationary,  and  based  on  its  best  guess  of  its  own  motion,  can  estimate  where  it  will 
see  the  landmarks  in  the  next  camera  frame.  The  difference  between  where  the  aircraft 
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sees  the  successive  object  images  and  where  it  expects  to  see  them  holds  the  key  to 
determining  the  error  in  the  INS.  For  example,  if  the  aircraft  thinks  it’s  rolling,  but  the 
horizon  doesn’t  change,  there  must  be  an  error  in  the  attitude  solution.  If  the  aircraft 
thinks  it  is  moving  very  fast,  but  the  objects  pass  by  slowly,  there  is  an  error  in  the 
velocity  solution.  This  process  of  Simultaneous  Location  And  Mapping  (SLAM)  has 
been  proposed  in  the  literature  [4][5][6][I  I],  and  is  the  basis  for  the  research  herein. 

If  so  much  information  can  be  gleaned  from  the  imagery,  it’s  fair  to  ask  why  the 
INS  is  needed  at  all.  Firstly,  the  INS  is  needed  to  predict  where  to  look  for  the  objects  in 
the  next  frame.  This  reduces  the  computational  burden  of  search  for  a  match.  Secondly, 
images  are  not  perfect.  They  contain  errors,  the  tracking  algorithms  induce  errors,  and 
two-dimensional  images  are  limited  in  what  information  can  be  reliably  obtained  about  a 
three-dimensional  world.  Kalman  filtering  allows  the  input  from  an  error-prone  INS  to 
complement  the  input  from  imperfect  image-based  estimations.  An  INS  drifts  over  time, 
but  gives  precise  rate  and  rotation  information.  Tracked  landmarks  do  not  drift  over  time, 
but  still  images  provide  poor  rate  information,  complementing  the  INS.  The  resultant 
navigation  solution  builds  upon  the  strengths  of  both  components,  and  diminishes  the 
weaknesses.  The  result  is  an  aided  INS  output  with  bounded  error  characteristics. 

Image  aiding  an  INS  promises  to  provide  GPS  quality  precision.  It  is  true  that  the 
benefit  of  tracking  a  single  landmark  does  not  compare  with  the  precision  of  a  GPS 
solution.  However,  image  aiding  allows  the  aircraft  to  track  many  landmarks  and  take 
updates  at  the  frame  rate  of  the  camera  being  used.  This  is  dependent  on  the  processing 
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rate  of  the  computer,  of  course,  but  as  computers  become  faster,  this  technology  becomes 
more  beneficial. 

1.1.9  Low  Cost  Inertial  Navigation.  Inexpensive  systems  like  UAVs  and  the 
JDAM  employ  inexpensive  INS  units  [30].  Low-cost  inertial  sensors  are  pseudonymous 
with  low  quality.  These  systems  prove  to  be  very  unstable  and  require  much  external 
aiding  to  be  of  any  use.  In  recent  years,  even  lower-cost  micro-machined 
electromechanical  systems  (MEMS)  have  been  developed  [23].  They  perform  even  more 
poorly  without  external  aiding,  but  are  very  appropriately  priced  for  such  applications. 
GPS  has  successfully  been  used  to  aid  a  MEMS  INS,  delivering  acceptable  performance 
for  a  UAV  application  [3].  Unfortunately,  denial  of  GPS  would  render  MEMS  INS 
completely  useless.  A  typical  EGI  system  has  slow  enough  drift  characteristics  that  it  can 
withstand  rather  long  GPS  outages.  A  MEMS  unit  could  diverge  in  about  a  minute  or 
less,  causing  a  UAV  to  crash,  or  a  JDAM  to  miss.  Image  aiding  provides  a  passive, 
robust  solution  and  has  proven  to  provide  near-GPS  level  precision  [1].  Applying  image 
aiding  to  a  MEMS  unit  will  give  precision  at  a  fraction  of  the  cost  of  a  navigation-grade 
INS,  and  without  the  vulnerabilities  of  an  EGI.  This  research  aims  to  extract  equivalent 
benefit  from  much  lower-cost,  and  therefore,  more  appropriate  INS  systems  for  UAVs 
and  disposable  weapons. 
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1.2  Problem  Statement 


Previous  work  on  optical  inertial  navigation  demonstrated  its  potential,  but  leaves 
many  questions  unanswered.  These  questions  include:  what  are  the  true  nature  and 
practical  limitations  of  such  a  system  in  a  full-scale,  operationally  representative 
environment?  This  research  will  develop  a  rigorous  theoretical  observability  analysis  to 
characterize  the  nature  of  optical  aiding  in  order  to  design  a  robust  algorithm.  Simulation 
will  be  used  to  draw  conclusions  and  enhance  design.  Flight  test  of  a  monocular  system 
incorporating  a  low-cost  MEMS  INS  will  be  used  to  evaluate  the  theory.  The  effects  of 
using  pre-surveyed  landmarks,  unknown  landmarks,  and  a  mixture  of  both  will  be 
evaluated  as  well. 


1.3  Scope  and  Assumptions 

Much  research  and  study  has  been  done  in  the  field  of  optical-inertial  navigation 
(as  described  in  the  next  Section).  This  research  focuses  on  the  single-camera  monocular 
case.  Additionally,  the  flight  environment  tested  will  be  representative  of  real-world 
aircraft,  and  not  limited  to  small-scale  models.  A  rigorous  mathematical  study  of  the 
influencing  factors  affecting  performance  will  be  accomplished  and  validated  with  test 
data.  The  end  product  goal  is  a  fieldable  algorithm  that  accounts  for  practical  system 
limitations  and  does  not  rely  on  artificial  assumptions. 

This  research  does  not  explore  the  use  of  active  sensors,  nor  purpose-built 
landmarks  which  have  proven  to  solve  many  of  the  image  processing  challenges  [1].  The 
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image  processing  side  of  the  problem  is  not  the  subject  of  this  research,  but  is 
accomplished  with  a  readily  available  feature  generation  algorithm.  The  image 
processing  is  not  perfect,  but  practical  techniques  to  improve  it  for  this  application  are 
addressed.  The  research  herein  concentrates  on  the  navigation  aiding  component  of  the 
problem,  specifically  as  it  pertains  to  contemporary  aviation. 


1.4  Related  Research 

The  idea  of  using  imagery  to  aid  navigation  is  nothing  new.  This  research  will 
follow  on  a  previous  master’s  thesis  [6],  and  recent  PhD  work  [26]  accomplished  at 
AFIT.  Much  work  has  also  been  done  in  private  industry  in  this  particular  field  [4]  [23]. 
The  research  herein  will  explore  a  specific  corner  of  this  vast  field  of  study.  The 
following  is  a  survey  of  work  related  to  image  aided  navigation.  This  section  aims  to 
compare  and  contrast  the  current  state  of  the  art  with  the  original  research  represented  in 
this  thesis.  The  goal  is  to  establish  the  validity  of  the  work  herein  as  well  as  distinguish  it 
as  original  and  noteworthy. 

1.4.1  Vision  to  Generate  Relative  Navigation  Commands.  One  successful 
approach  to  vision-based  navigation  involves  using  video  to  generate  relative  navigation 
commands.  These  commands  are  fed  into  a  GPS-aided  INS  for  the  purposes  of  landing  a 
UAV.  The  AVATAR  project  is  a  radio  controlled  helicopter  equipped  with  a  Differential 
GPS  (DGPS)  onboard  to  provide  absolute  position  of  the  aircraft  [24].  It  uses  a  CCD 
camera  to  find  a  helipad  and  land.  The  helipad  is  of  a  known  shape  and  size  and  made  to 
have  a  high  contrast  to  the  surrounding  environment.  The  high  contrast  nature  of  the 
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helipad  allows  the  AVATAR  to  filter  out  the  surroundings  and  ealeulate  its  relative 
position.  This  researeh  has  proven  very  sueeessful,  but  differs  from  the  nature  of  the 
researeh  herein  in  many  ways.  Firstly,  the  AVATAR  relies  on  DGPS  to  solve  its  own 
position  and  attitude;  the  vision  algorithms  provide  nothing  in  the  way  of  aiding  the  INS. 
Additionally,  the  helipad  is  a  single  landmark  of  known  dimensions,  simplifying  the 
problem  of  determining  the  relative  position  to  the  landing  zone.  Lastly,  the  aireraft 
used  was  a  helieopter.  The  researeh  herein  will  traek  many  unknown  landmarks,  with 
little  to  no  a  priori  information.  The  landmarks  will  aid  the  INS  preeision  and  not  be  used 
direetly  for  relative  guidanee  eommands.  Additionally,  the  aireraft  to  be  used  will  be 
fixed-wing,  exhibiting  vastly  different  flight  dynamies  from  those  of  a  helieopter. 

1.4.2  Image  Aiding  with  A  Priori  Information.  A  group  from  the  Georgia 
Institute  of  Teehnology  used  a  small  helieopter  to  implement  a  vision- aided  INS  [I]. 
This  group  put  an  INS  on  a  small  GTMax  helieopter  and  aided  it  with  video  output  from 
a  body-mounted  eamera  [8].  Through  the  use  of  an  extended  Kalman  filter,  the  INS 
position  and  attitude  were  aided  without  the  need  of  GPS.  Simulations  were  very 
promising;  however,  the  experiment  laeked  a  truly  robust  solution.  The  simulation 
traeked  a  single,  vertieal  reetangle  of  known  dimensions  and  position.  This  a  priori 
information  simplified  the  problem  of  updating  the  onboard  INS,  but  the  algorithms  are 
essentially  the  same  as  will  be  diseussed  herein. 

Members  of  this  same  group  explored  using  vision  as  a  means  of  determining 
position  relative  to  a  runway  [1].  Beaeons  were  plaeed  on  the  four  eomers  of  a  runway  of 
known  dimension.  The  measurements  of  these  beaeons  (as  seen  in  the  video)  were  used 
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as  the  sole  inputs  to  a  Kalman  filter  determining  position  and  attitude.  The  simulation 
results  indicate  that  vision  alone  may  be  an  acceptable  solution  for  the  UAV  landing 
problem.  These  results  using  a  single  set  of  known  landmarks  (four  corners  of  a 
rectangle)  are  promising.  The  research  herein  will  explore  tracking  many  more 
landmarks  with  and  without  a  priori  information  (including  a  landing  task),  but  the 
Georgia  Institute  of  Technology  study  adds  validation  to  the  idea  of  extracting  navigation 
information  from  imagery. 

1.4.3  Image  Aiding  with  No  A  Priori  Information.  A  process  called 
Simultaneous  Localization  And  Mapping  (SLAM)  has  been  recently  researched  at  the 
University  of  Sydney,  Australia  [10] [5].  This  process  uses  video  streams  to  “mo/?” 
terrain  according  to  landmarks  of  interest.  These  landmarks  are  described  as  “features... 
that  can  be  consistently  and  reliably  observed”  by  the  aircraft.  No  a  priori  information  is 
known  about  the  location  of  the  landmarks.  However,  the  image  processing  and  feature 
tracking  problem  is  aided  with  knowledge  about  the  feature’s  appearance.  By  tracking 
the  observed  landmark  position  in  sequential  video  frames  and  comparing  with  estimated 
positions  through  an  extended  Kalman  filter,  a  3-dimentional  map  can  be  made  of  the 
surrounding  terrain.  These  tracked  landmarks  can  then  be  used  to  update  an  INS  in 
position  and  attitude.  Simulation  showed  impressive  results;  aircraft  were  able  to  obtain 
near-GPS  quality  positions  and  attitudes  while  flying  a  SLAM  profile  circuit.  In  their 
research,  the  authors  also  describe  another  process  called  Terrain  Aided  Navigation 
System  (TANS),  which  is  very  similar.  The  difference  is  that,  with  TANS,  the  terrain  has 
been  previously  mapped  and  stored  in  a  database  accessible  to  the  INS.  This  reduces 
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uncertainty  in  the  measurements  and  inereases  performance,  but  relies  on  a  priori 
information.  The  researeh  herein  relies  on  a  similar  SLAM-like  algorithm  to  generate 
landmarks.  While  the  image  proeessing  side  of  the  SLAM  algorithm  is  not  the  focus  of 
this  thesis,  loeating  and  traeking  landmarks  is  a  large  part.  These  traeked  landmark 
loeations  beeome  part  of  the  overall  state  veetor  in  the  Kalman  filter  in  order  to  aid  the 
INS.  In  essenee,  the  SLAM  algorithm  allows  the  aircraft  to  develop  its  own  “a  priori” 
information  for  landmarks  it  finds  along  the  way.  The  University  of  Sydney  study  gives 
eredibility  to  a  pivotal  concept  on  whieh  this  thesis  depends.  This  researeh  will  show  that 
the  environments  a  military  aircraft  will  encounter  pose  signifieant  challenges  not 
addressed  in  the  work  deseribed  above,  and  solutions  to  these  ehallenges  will  be  posed. 

1.4.4  Image  Aiding  an  EGI  during  GPS  Drop  Outs.  The  NAVSYS  Corporation 
has  recently  been  researehing  the  use  of  image-based  updates  to  assist  EGI  systems 
during  periods  of  GPS  denial  [4][23].  The  prineiples  applied  are  very  similar  to  those 
used  herein:  two-dimensional  landmarks  are  tracked  to  update  the  inertial  navigation 
solution.  Three-dimensional  objeet  models  were  generated  of  suitable  landmarks  in  the 
experimental  flight  area.  These  models  were  loealized  through  multiple  images  and  a 
target  geo-loeation  algorithm  while  GPS  was  available  to  the  EGI  [23].  The  model 
database  included  a  three-dimensional  position  for  each  landmark  and  distinct  image 
attributes.  This  data  bank  of  landmarks  served  to  update  the  EGI  when  GPS  was  later 
denied  during  the  flight  test.  The  stored  image  attributes  helped  to  find  and  identify  the 
landmarks,  while  the  three-dimensional  position  provided  a  sort  of  a  priori  information. 
Update  rates  of  1-2  per  minute  proved  suffieient  for  short  periods  of  GPS  denial.  The 
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experimental  results  showed  great  promise  for  image  aiding  INS  in  the  manner  proposed 
herein. 

The  NAVSYS  work,  although  very  similar,  differs  quite  significantly  from  this 
thesis.  Firstly,  NAVSYS  relied  heavily  on  a  priori  information  gathered  while  GPS  was 
available.  This  research  will  attempt  to  rely  on  landmarks  of  opportunity  (LOO)  that 
have  not  been  previously  surveyed  (no  a  priori  information).  Additionally,  the  NAVSYS 
process  requires  that  the  surveyed  landmarks  be  revisited  during  the  GPS  outages.  The 
aircraft  also  maintained  the  same  altitude  during  the  entire  flight.  These  assumptions  are 
not  robust  enough  for  dynamic  profiles  that  military  applications  may  undergo.  Lastly, 
the  periods  of  GPS  denial  were  relatively  short,  while  this  thesis  will  explore  the 
performance  of  image  aiding  during  the  course  of  an  entire  flight  profile.  Although 
different  in  many  important  ways,  the  NAVSYS  study  validates  to  the  use  of  two- 
dimensional  images  to  aid  INS. 

1.4.5  Stochastic  Image  Correspondence.  In  order  to  have  any  benefit,  the 
location  of  landmarks,  within  successive  images,  must  be  a  function  of  the  inertial  state 
of  the  aircraft  taking  the  pictures.  Relating  landmarks  in  different  images  is  termed  the 
correspondence  problem,  and  can  be  simplified  by  using  inertial  navigation  information. 
Combining  conventional  epipolar  correspondence  geometry  with  stochastic  projection 
modeling,  Veth,  Raquet,  and  Pachter  were  able  to  decrease  the  uncertainty  projection  of 
landmarks  significantly  in  subsequent  image  frames  [28].  The  reduced  uncertainty 
decreases  the  processing  cost  of  feature  tracking  algorithms  that  are  critical  to  image- 
aided  INS.  This  research  is  important  because  it  characterizes,  stochastically,  the  image 
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correspondence  problem  upon  whieh  image  aiding  rests.  Because  the  landmark 
projection  is  modeled  stochastically,  and  the  INS  is  updated  via  an  extended  Kalman 
filter,  superior  performance  can  be  expeeted  over  any  other  method  [14][15].  This 
stochastie  model  of  the  landmark  projeetion  will  be  ineorporated  into  the  researeh  herein. 

1.4.6  Experimental  Validation  -  Peeping  Talon.  Giebner  and  Raquet  performed 
an  image  aided  INS  flight  test  at  the  USAF  TPS  in  2002,  nieknamed  ''Peeping  Talon’’’ 
[6].  A  USAF  T-38  was  equipped  with  a  Honeywell  H-764G  Embedded  GPS/INS  (EGI), 
two  video  cameras,  and  two  Ashteeh  Z-surveyor  semi-codeless  receivers  to  provide  truth 
data.  The  experiment  eolleeted  inertial  measurements  from  the  H-764G  EGI  and  imagery 
from  the  two  cameras  as  it  orbited  over  a  predetermined  target  area.  A  small  number  of 
features  were  chosen  to  be  landmarks,  and  manually  traeked  through  the  video  frames 
(feature  traeking  algorithms  were  not  employed).  The  inertial  measurements  and  the 
manual  video  measurements  were  fed  into  a  feed-forward  extended  Kalman  filter,  and 
compared  to  the  truth  data  obtained  by  the  Ashteeh  receivers. 

The  experimental  results  showed  that  image  aiding  could  deliver  close  to  GPS 
quality  performanee.  This  researeh  validates  the  effectiveness  of  image  aiding  aboard 
USAE  aircraft  carrying  USAE  INS,  but  leaves  mueh  to  expand  upon.  Eirst  of  all,  feature 
traeking  was  accomplished  manually.  This  induces  some  man-made  errors  and  reduces 
some  maehine-made  errors  inherent  to  feature  traeking  algorithms.  Seeond,  the  inertial 
measurements  were  taken  from  the  relatively  high  quality  H-764G.  Third,  the  flight  path 
varied  little,  and  revisited  the  same  set  of  landmarks  over  and  over.  Eourth,  the  eamera 
used  looked  out  the  side  of  the  aircraft,  where  this  test  will  look  along  the  longitudinal 
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axis.  Lastly,  only  a  small  number  of  landmarks  were  used,  and  the  geometry  of  these 
landmarks  varied  little  over  the  flight. 

The  research  herein  will  expand  upon  the  Peeping  Talon  work,  in  order  to  provide 
a  more  robust  solution  to  the  fore-mentioned  military  applications.  First,  an  online 
feature  tracker  will  be  implemented  and  modeled.  Many  more  features  will  be 
automatically  identified  and  simultaneously  tracked.  The  inertial  measurements  will  be 
taken  from  free-running  MEMS  and  low-quality  INS  units.  Last,  experimental  flight 
trajectories  will  model  appropriate  UAV  and  weapons  profiles,  and  not  allow  for  the 
artificial  revisiting  of  landmarks.  The  thrust  of  this  thesis  will  be  to  take  concepts  proven 
in  research  like  Peeping  Talon,  and  develop  it  for  practical  applications. 

1.4.7  Genesis  of  this  Research.  This  research  effort  is  an  extension  of  work 
accomplished  as  an  AFIT  PhD  dissertation  in  2005  [26].  Veth  successfully  developed  an 
end-to-end  optical-inertial  navigation  system  that  employed  binocular  vision.  Borrowing 
the  Scale  Invariant  Feature  Transform  (SIFT®  [II])  to  accomplish  the  image  processing, 
Veth  employed  an  extended  Kalman  filter  to  accomplish  the  INS  aiding.  He  solved  the 
computational  burden  of  feature  matching  and  correspondence  by  constraining  the  search 
with  estimates  derived  by  the  INS  [28].  The  system  was  built  around  a  MEMS-grade 
inertial  measurement  unit  (IMU)  and  two  digital  still  cameras.  The  use  of  binocular 
vision  enabled  3-dimensional  resolution  from  2-dimensional  images.  Veth  accomplished 
meter  level  position  precision  in  indoor  environments,  and  flight  test  data  showed  similar 
promise,  but  lacked  a  faithful  truth  source.  The  system  developed  by  Veth  is  very  robust 
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and  functional,  and  it  truly  incorporates  zero  a  priori  navigation  with  no  artifieialities  that 
skew  analysis. 

The  system  used  in  this  researeh  ineorporates  the  same  MEMS  IMU,  feature 
generation  algorithm,  eameras  and  underlying  algorithmie  arehiteeture  used  by  Veth,  but 
eoneentrates  on  the  monoeular  ease.  Additionally,  the  analysis  in  this  researeh  benefits 
from  preeision  truth  data,  whieh  was  not  available  to  Veth  at  the  time.  Veth’s  work 
proved  that  a  praetieal  SLAM-based  algorithm  ean  in  fact  provide  preeision  navigation  in 
dynamie  environments,  validating  the  researeh  herein.  Building  upon  his  work  allows  the 
extension  to  the  military  aviation  environment.  This  environment  exhibits  ehallenges  not 
encountered  in  the  indoor  ease  or  at  low  altitude  environments  discussed  to  this  point. 


1.5  Methodology 

This  thesis  begins  with  the  mathematieal  baekground  needed  to  understand 
inertial  navigation,  Kalman  filtering  and  basie  opties  prineiples.  Inertial  navigation 
theory  relies  on  kinematics  and  the  basic  laws  of  physics,  which  are  described  in  detail  in 
Chapter  2.  A  preeise  earth  model  is  developed,  as  well  as  the  charaeterization  of  inertial 
sensors.  The  INS  model  required  for  aiding,  and  how  it  relates  to  Kalman  filtering,  are 
deseribed. 

Chapter  3  develops  the  equations  that  relate  the  optieal  measurements  to  the  INS 
states.  These  equations  are  then  linearized  for  use  in  the  extended  Kalman  filter.  A 
rigorous  study  of  the  observability  of  these  measurement  equations  is  aceomplished  in  an 
effort  to  design  an  optimal  algorithm.  The  nature  of  surveyed  landmarks  and  landmarks 
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of  opportunity  is  explored.  A  set  of  prineiples  for  optieal  aiding  design  is  developed, 
leading  to  the  final  algorithm. 

Chapter  4  deseribes  how  simulation  was  used  to  validate  and  generate  the  flight 
test  algorithm  and  apparatus.  The  flight  test  program  is  deseribed  from  a  teehnical  design 
and  ealibration  standpoint.  The  flight  test  data  points  were  ehosen  to  isolate  and  evaluate 
the  factors  influencing  performance  (derived  in  Chapter  3).  Characterization  of  the 
performance  is  accomplished  through  statistical  rigor.  Characterizations  will  be  made  by 
examining  the  statistical  performance  over  many  data  events,  and  not  on  single  best  case 
data  runs. 

Finally,  flight  test  data  will  be  used  to  form  conclusions  about  the  nature  of 
optical-inertial  navigation.  The  system  under  test  (SUT)  will  be  evaluated  and 
recommendations  for  improvements  made.  Future  areas  of  study  will  be  also 
recommended,  based  on  flight  test  shortcomings  or  predicted  areas  of  fruitful  study. 
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II.  Mathematical  Background 


This  chapter  describes  the  mathematics  and  physics  required  to  understand  inertial 
navigation  and  the  associated  aiding  process.  It  describes  the  notation  and  models  to  be 
used  throughout  this  research,  as  well  as  the  physical  dynamics  driving  the  process.  Error 
analysis  tools  will  also  be  described. 

This  chapter  establishes  the  required  notation,  frames  of  reference,  basic  Earth 
model,  inertial  sensor  models,  and  Inertial  Navigation  System  (INS)  mechanization.  A 
development  of  linearized  systems  and  extended  Kalman  filters  (EKE)  follows,  enabling 
INS  aiding.  Easily,  the  optics  model  is  described.  The  bulk  of  this  discussion  is  derived 
from  the  work  of  Tritteron  and  Weston  on  inertial  navigation  [23],  Maybeck  on  Kalman 
filtering  [14] [15],  and  Veth  on  inertial  optical  fusion  [26]. 


2.1  Mathematical  Notation 

The  notation  used  herein  is  described  below: 

•  Scalars:  upper  or  lower  case  italic  letters,  (e.g.,  a  or  ^4). 

•  Vectors:  lower  case  bold  letters,  (e.g.,  x).  All  vectors  are  column  vectors;  Xi  is 

the  scalar  component  of  x  in  the  row  of  x. 

•  Unit  Vectors:  denoted  with  the  cAeck  character  (e.g.,  X).  Unit  Vectors  have  unit 

length  defined  by  the  two  norm  (e.g.,  ||x||  =  1 ). 
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•  Matrices:  upper  case  letters  in  bold  font,  (e.g.,  X)  Xy  is  the  scalar  component  of 

X  in  the  row  and column. 

•  Transpose:  a  vector  or  matrix  transpose  is  indicated  by  a  superscript  T  (e.g., 

orX^). 

•  Estimated  variables:  variables  which  are  estimates  of  random  variables  are 

denoted  by  the  hat  character  (e.g.,  x  ). 

•  Computed  Variables:  variables  which  are  computed  and  therefore  corrupted  by 

errors  are  denoted  by  the  tilde  character  (e.g.,  x  ). 

•  Measured  Variables:  variables  which  are  measured  and  therefore  corrupted  by 

errors  are  denoted  by  the  bar  character  (e.g.,  x  ). 

•  Homogeneous  Coordinates:  vectors  in  homogeneous  coordinate  form  are 

denoted  by  an  underline  (e.g.,  x).  Homogeneous  coordinate  vectors  have  a 
value  of  1  in  the  last  element. 

•  Direction  Cosine  Matrices  (DCM):  a  DCM  that  transforms  from  frame  a  to 

frame  b  is  denoted  by  C* . 

•  Reference  Frame:  a  vector  expressed  in  a  particular  reference  frame  is  denoted 

by  a  superscript  letter  referring  to  said  frame  (e.g.,  x“  is  expressed  in  the  a- 
frame) 

•  Relative  Position  or  Motion:  positions  and  motions  relative  to  two  different 

frames  are  denoted  by  two  subscript  letters  corresponding  to  the  two  frames  of 
interest,  (e.g.,  represents  the  rotation  rate  vector  of  b  relative  to  a.) 
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2.2  Reference  Frames 

Inertial  navigation  and  aiding  requires  an  understanding  of  the  referenee  frames 
used  to  express  3-dimensional  veetors  and  measurements.  The  referenee  frames  used 
herein  are  three-dimensional  Cartesian  frames  with  an  orthonormal  basis  in9l\  deseribed 
below: 

2.2.1  True  Inertial  Frame  (I-frame).  Theoretieal  frame  where  Newton’s  laws 
apply.  It  is  non-aeeelerating  and  non-rotating,  but  has  no  defined  origin  (due  to 
relativity). 

2.2.2  Earth-Centered  Inertial  Frame  (i-frame).  This  frame  has  an  origin  at  the 
eenter  of  the  Earth.  The  z-axis  points  to  the  North  Star.  The  x-axis  falls  on  the  equatorial 
plane,  and  points  to  the  first  star  of  Aires.  The  y-axis  is  orthogonal  to  both  and  falls  on 
the  equatorial  plane.  This  frame  is  non-rotating,  but  does  move  with  the  Earth  as  the 
Earth  rotates  about  the  sun  (and  the  sun  about  the  galaxy).  Eor  the  purposes  of  navigation 
on  the  Earth,  the  i-frame  ean  be  approximated  as  a  true  inertial  frame  (due  to  the  slow 
rate  of  the  Earth  about  the  sun,  ete). 

2.2.3  Earth-Centered  Earth-Fixed  Frame  (e-frame  or  ECEF  frame).  Eike  the  i- 
frame,  the  e-frame  has  its  origin  at  the  center  of  the  Earth,  but  it  does  rotate  with  the 
Earth;  it  is  fixed  to  the  planet.  The  z-axis  aligns  with  the  North  pole.  The  x-axis  falls  on 
the  equatorial  plane,  and  points  to  the  Greenwich  meridian.  The  y-axis  again  falls  on  the 
equatorial  plane,  and  points  to  90  degrees  east  longitude. 

2.2.4  Navigation  Frame  (n-frame).  This  is  a  local  geographic  frame,  with  its 
origin  at  the  location  of  the  navigation  system.  The  x  and  y-axis  point  to  North  and  East, 
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respectively.  The  z-axis  points  down.  Down  is  described  by  the  direction  of  the  gravity 
vector  for  a  given  location  on  the  Earth.  This  North-East-Down  (NED)  convention  will 
be  used  in  other  frames  as  well.  See  Eigure  2.1  for  illustrations  of  the  i.e.  and  n-frame. 

2.2.5  Body  Frame  (b-frame).  This  frame  is  rigidly  attached  to  the  vehicle.  The 
origin  of  the  b-frame  is  co-located  with  the  n-frame,  but  the  axes  rotate  with  the  aircraft. 
The  X,  y  and  z-axis  point  out  the  nose,  right  wing,  and  bottom  of  the  aircraft,  respectively. 
See  Eigure  2.2. 

2.2.6  Camera  Frame  (c-frame).  This  frame  is  rigidly  attached  to  the  camera. 
The  origin  is  at  the  camera’s  optical  center.  The  x  and  y-axis  point  up  and  right, 
respectively.  The  z-axis  points  out  the  camera  lens,  perpendicular  to  the  focal  plane.  It  is 
useful  to  say  that  the  x  and  y-axis  align  to  the  camera’s  detector  array  (discussed  later). 
See  Eigure  2.3. 
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Figure  2,1:  Earth-Centered  Inertial,  Earth-Centered  Earth-Fixed,  and  Navigation 
Frame.  The  Earth  Fixed  and  Earth-Centered  Earth-Fixed  frames  originate  at  the 
Earth’s  center  of  mass,  while  the  navigation  frame  is  fixed  to  a  vehicle  [26], 


Figure  2.2:  Aircraft  body  frame  illustration.  The  origin  of  the  b-frame  is  co-located 
with  the  n-fmme,  hut  the  axes  rotate  with  the  aircraft  [26]. 
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Figure  2,3:  Camera  frame  illustration.  The  camera  reference  frame  originates  at 
the  center  of  the  focal  plane  [26], 


An  understanding  of  reference  frames  is  important  to  express  vectors  properly. 
The  transformation  between  reference  frames  can  now  be  addressed. 
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2.3  Transforms  Between  Reference  Frames 

Any  vector  can  be  expressed  (or  resolved)  in  any  of  the  frames  described.  If  a 
vector  is  resolved  in  one  frame,  it  will  have  the  superscript  corresponding  to  that  frame. 
See  Figure  2.4  below. 


n-frame 


n-frame 


h-frame 

A 


Figure  2,4:  Expressing  vectors  in  different  frames.  The  same  vector  can  take  on 
different  expressions  when  resolved  in  different  frames.  The  magnitude  is 
unaffected. 


To  express  a  vector  in  one  frame,  when  given  a  vector  resolved  in  another,  a 
Direction  Cosine  Matrix  (DCM)  can  be  used  to  perform  a  coordinate  transformation.  The 
DCM  can  be  generated  from  a  series  of  Euler  angles  to  perform  an  ordered  set  of  single 
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axis  rotations  on  the  vector  of  interest.  Euler  angles  are  elements  of  a  three-parameter 
vector  corresponding  to  a  specific  sequence  of  single-axis  rotations  to  transform  from  one 
reference  frame  to  another.  Commonly,  Euler  angles  express  the  transformation  from  the 
navigation  to  body  frame  of  an  aircraft:  yaw,  pitch  and  roll.  Performing  the  rotations  in 
this  order  is  known  as  the  3-2-1  convention  [23].  This  convention  will  be  used  herein. 

Direction  Cosine  Matrices  are  four-parameter  transformations  expressed  as  a  3x3 
matrix  [23].  The  matrix  consists  of  the  inner  product  (or  Cosines)  of  each  basis  unit 
vector  in  one  frame  with  each  basis  unit  vector  in  another  frame.  The  DCM  is  used  to 
transform  vectors  resolved  in  one  frame  to  another,  as  seen  below: 

v'’=cy  (2.1) 

When  used  to  transform  right-hand  Cartesian  coordinates,  the  DCM  has  the 
following  properties: 


c:=(c:f  =(c‘)'  (2.2) 


c;c 


b 

a 
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When  a  DCM  changes  over  time,  due  to  the  frames  of  interest  rotating  with  respect  to 
each  other,  the  DCM  satisfies  the  following  differential  equation  [23]; 


C:=C:QL  (2.3) 

where  is  the  skew  symmetric  form  of  the  rotation  vector  from  frame  b  to  a,  resolved 
in  a  ( (ol^ ).  The  skew  symmetric  operator  of  a  vector  is  defined  by  co  x : 


CO 

"  0 

-CO 

CO 

X 

z 

y 

CO  = 

COy 

G)X  = 

0 

_^zj 

0 

Direction  Cosine  Matrices  are  essential  to  apply  when  dealing  with  vectors 
resolved  in  different  frames.  Vectors  from  different  frames  cannot  be  used  together,  until 
they  are  transformed  into  the  same  reference  frame.  With  an  understanding  of  how 
vectors  are  expressed,  the  process  of  modeling  physical  systems  can  now  be  addressed. 
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2.4  Physical  System  Modeling 

Physical  systems  and  their  assoeiated  dynamies  equations  ean  often  be  written 
and  arranged  into  eonvenient  matrix  form.  This  allows  the  use  of  linear  algebra  to 
aeeomplish  mueh  of  the  modeling  needed  in  INS  applieations.  The  dynamies  herein  are 
modeled  as  systems  of  linear  differential  equations. 

2.4.1  Linear  Systems.  Many  physieal  systems  ean  be  modeled  sueh  that  they  fit 
the  format  of  a  linear  system  of  equations  [14];  shown  below. 

x(0  =  F(0x(0  +  B(0u(0;  x(to)  =  Xo  (2.5) 

In  the  linear  time -varying  ease,  F(t)  and  B(t)  are  time -varying  matriees.  For  a  given  set 
of  initial  eonditions  (x^)  and  input  (u(t)),  Equation  (2.5)  has  a  unique  solution 
trajeetory,  x(t)  [14];  given  by: 

t 

x(t)  =  0(tTo)XQ  -i-|o(t,T)B(T)u(T)JT  (2.6) 


The  state  transition  matrix  0(f  t^)  takes  the  state  x(t)  from  time  tg  to  time  t  for 

the  homogeneous  ease  (Equation  (2.6)  without  the  last  term  on  the  right  hand  side),  and  is 
defined  by: 

^£^^  =  F(0®(ao);  ^{to,h)  =  \  (2.7) 
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Significant  properties  of  O(tTo)  include: 

(2.8) 

(2.9) 

(2.10) 

In  the  linear  time-invariant  case,  where  F(t)  =  F  ,  t  e  91 

=  0(0)  =  I 

at 

(2.11) 

0(tTo)  =  0(Wo)  =  e''‘“"^ 

(2.12) 

2.4.2  Non-Linear  Systems.  Most  physical  systems  do  not  allow  for  linear 
models.  In  the  non-linear  case,  the  physical  system  can  often  be  expressed  as  the 
nonlinear  differential  equation  [15];  shown  below. 

x(0  =  f[x(0,u(0,t];  x(to)  =  Xo  (2.13) 

where  f[x(t),u(t),t]  is  a  vector  of  functions  with  arguments  including  the  state  vector 
(x(t)),  the  input  vector  (u(t)),  and  time  (t).  The  non-trivial  solution  x(t)is  the  result  of 
a  nonlinear  differential  equation. 
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Linear  properties  may  be  retained  by  making  linear  approximations  of  the 
nonlinear  solution,  about  nominal  trajeetory  values.  For  small  changes  about  a  nominal 
trajectory,  many  systems  can  be  well  modeled  to  first  order  through  the  use  of  the  Taylor 
Series  expansion  [15]  shown  below. 


[x(0-x„(0] 


af[x,u(0,t] 

dx 


lx=x„(t) 


[xit)-x„it)]  +  H.O.T. 


(2.14) 


where  x^(t)  is  the  nominal  trajectory  about  which  the  linearization  occurs.  Note  that 
u(t)  is  assumed  to  be  unperturbed  (u(t)-u„(t)  =  0),  thus  eliminating  the  partial  with 
respect  to  u(t).  If  the  system  is  well  modeled  to  first  order,  the  higher  order  terms 
(H.O.T.)  may  be  neglected  [15].  When  x{t)  is  redefined  to  be  a  perturbation  about  the 
nominal, 

x(0  =  x„(0  +  5x(0  (2.15) 

Equation  (2.14)  is  approximated  to  first  order  as: 


5x(t)  = 


5f[x,u(0,t] 


dx 


8x{t) 


x=x„(t) 


(2.16) 


or 


5x(0  =  F[t;x„(0]6x(0 


(2.17) 


where  F[t;x^(t)]  is  the  matrix  of  the  partial  derivatives  of  f[x,u(t),t]  [15]: 
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F[nu(0,x,(0] 


af[x,u(o,^] 

dx 


lx=Xn(0 


■■  ^ 

dx„ 

dx, 

1 

s: 

(2.18) 


In  error  analysis,  the  estimated  value  of  the  state  veetor  ( x{t)  )  is  equal  to  the  true 
value  of  the  state  veetor  ( x(t) )  plus  some  error  ( 5x(t) ). 

x(0  =  x(0  +  6x(0  (2.19) 

If  Equation  (2.19)  is  substituted  for  Equation  (2.15),  Equation  (2.17)  beeomes  a  nonlinear 
differential  equation  for  the  error  ( 5x(t) )  in  the  state  estimate  ( x(t) ),  linearized  about  the 
true  state  ( x(t) ). 

In  the  ease  where  u(t)  is  not  known  (i.e.  0 ),  the  error  (5u(t) )  ean 

be  modeled  as  well  by  linearizing  about  a  nominal  input  trajectory.  However,  this 
compromises  the  validity  of  the  small  perturbation  approximation,  used  in  Equation 
(2.16)  [15].  If  5u(t)  cannot  be  approximated  to  0  for  all  time,  then  it  can  be  modeled  as 
an  error  state  according  to  Equations  (2.20)(2.21)(2.22). 

5x(0  =  F[t;  x„  (0,  u„  (0]6x(0  +  B[t;  x„  (t),  u„  (0]5u(0  (2.20) 


31 


F[nx„(0,u„(0] 


dx 


lx=Xn(0,U=U„(iV 


B[^;x„(0,u„(0] 


^[x,u,^] 

5u 


lx=Xn(0,U=Un(0,« 


Ml 

Ml 

dxj 

Ml 

Mll 

dXj 

dx„ 

Ml 

duj 

Ml 

du, 

(2.21) 


(2.22) 


With  the  higher  order  derivatives  negleeted,  5x(t)  beeomes  the  solution  to  the 
time-varying,  linear  differential  Equation  (2.20).  Given  a  pieeewise-eontinuous 
F[t;x^(t),  11^(0]  and  B[t;x^(t),u„(t)]0u(t) ,  5x(t)  has  a  unique  solution  [15]; 

t 

8x(t)  =  0(t,tg)5Xg  +1 0(t,T)B(T)5u(T)JT  (2.23) 

where  B(t)  is  defined  by  Equation  (2.22),  and  0{t,tg)  is  defined  by  Equation  (2.7)  using 
F{t)  found  in  Equation  (2.21). 

These  non-linear  perturbation  models  will  be  used  when  implementing  the 
extended  Kalman  filter,  diseussed  later.  The  extended  Kalman  filter  will  be  used  to  aid 
the  Inertial  Navigation  System,  whieh  will  now  be  diseussed. 
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2. 5  Inertial  Navigation 

Inertial  navigation  is  accomplished  by  integrating  specific  force  and  angular  rate 
measurements  to  produce  an  estimate  of  the  navigation  state:  position,  velocity  and 
attitude.  Only  an  estimate  can  be  attained  due  to  imperfections  in  sensors  and 
approximations  of  the  true  system  dynamics.  This  section  will  cover  the  dynamics, 
mechanization,  and  error  propagation  of  an  INS. 

2.5.1  Shape  of  the  Earth.  In  order  to  navigate  on  the  Earth,  one  must  know  its 
shape.  There  are  many  models  for  the  Earth,  but  the  World  Geodetic  System  of  1984 
(WGS84)  model  will  be  used  for  this  research  [23].  WGS84  models  the  Earth  as  an 
ellipsoid,  wider  along  the  equator.  The  mean  radius  of  the  Earth  {Rq),  the  meridian 

radius  of  curvature  {Rff),  and  the  transverse  radius  of  curvature  {Re)  are  modeled  as: 


^0  “ 

R{\-e^) 


D  _ 

^  ’/1  „2  ^  - ^2  xx3/2 


(l-e"  siWiy 

R 


D  _  _ 

/-I  2  ^-2  xxl/2 


(l-e  siWiy 


(2.24) 

(2.25) 

(2.26) 


These  radii  are  functions  of  latitude  (L  ),  eccentricity  of  the  ellipsoid  model  (e),  and  the 
semi-major  axis  (R);  defined  by  WGS84: 

=  6378 1 37.0  meters  (2.27) 

e  =  0.0818191908426  (2.28) 
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2.5.2  Gravitational  Model.  Modeling  gravity  is  critical  to  successful  inertial 
navigation.  The  subtle  changes  in  the  gravitational  field  must  be  taken  into  account. 
The  most  obvious  factor  affecting  the  strength  of  the  gravitational  field  is  the  distance 
from  the  center  of  the  Earth.  For  this  research,  gravity  will  be  modeled  as  follows  [23]; 


g(0) 

{\  +  hlR^f 


(2.29) 


where  h  is  the  height  above  the  WGS-84  ellipsoid  (Earth  model),  is  the  Earth’s 
radius  defined  above,  and  g(0)  is  the  magnitude  of  gravity  at  the  ellipsoid  surface: 

g(0)  =  9.780318(l  +  5.3024xl0 'sm'E-5.9xlO^'sm'2E)  m//  (2.30) 

Centripetal  acceleration  due  to  the  rotation  of  the  Earth  affects  the  magnitude  and 
direction  of  the  effective  gravitational  field  at  every  point  on  the  Earth.  Figure  2.5 
illustrates  this  effect.  Focal  gravity  (g^)  becomes  a  function  of  the  actual  gravitational 

field  vector  (g),  the  Earth’s  rotation  vector  (co.^)  and  the  specific  position  on  the  Earth 
( p ),  and  is  defined: 


9/  =g-®,.x[“feXP] 


(2.31) 
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where  |g||  is  defined  by  Equation  (2.29).  The  n-frame  is  defined  tangent  to  the  ellipsoid. 

The  true  gravitational  field  varies  slightly,  but  the  differenee  between  down  defined  by 
the  true  gravitational  field  and  the  n-frame  is  small.  For  the  purposes  of  this  diseussion 
assume  that  the  gravitational  field  points  down  in  the  n-frame: 

g  =  [0  0  g(/i)f  (2.32) 


Figure  2,5:  Effects  of  centripetal  force  on  the  local  gravitational  field. 

For  terrestrial  navigation,  the  Earth’s  rotation  (sidereal  angular  rate)  ean  be 
modeled  as  constant  [23]; 

<=[0  0  fl„r  (2-33) 
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However,  this  rotation  vector,  resolved  in  the  n-frame,  is  a  function  of  latitude; 


<=[^.cosI  0  sin  if 


(2.34) 


where  i2.^and  L  are  the  scalar  Earth  sidereal  angular  rate  and  latitude  respectively. 
WGS84  defines; 


=  7.292115x10  VaJA 


(2.35) 


Combining  Equations  (2.31),(2.4)  and  (2.34)  yields  a  final  expression  for  local 
gravity,  resolved  in  the  n-frame\ 


g,  =g 


Qj(Ro+h) 


sin2L 

0 

1  +cos2L 


(2.36) 


2.5.3  Accelerometer  Sensor  Model.  INS  systems  typically  incorporate  a  triad  of 
accelerometers,  arranged  at  the  origin,  pointing  orthogonally  along  each  of  the  three  b- 
frame  axes.  Despite  the  name,  accelerometers  do  not  measure  acceleration  directly. 
Accelerometers  measure  specific  force.  Specific  force  is  the  sum  of  the  acceleration 
experienced  due  to  gravity,  and  the  inertial  acceleration  experienced  by  the  body  [23]; 


m= 


d'pjt) 

dt^ 


9/ 


(2.37) 
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where  f(^)  is  the  three-dimensional  vector  of  specific  forces  measured  by  the 
accelerometers.  The  position  of  the  b-frame  p(t)  is  resolved  in  inertial  space,  and  is 
the  local  gravitational  vector  at  the  specific  location  p(t)  . 

In  order  to  be  used  by  an  INS,  these  accelerometers  must  be  modeled  and  their 
errors  characterized.  The  simplified  accelerometer  model  used  herein  has  the  specific 
force  measurement  corrupted  by  a  bias  and  an  additive  white  Gaussian  noise  [26]. 

f'’ =f+a'’+w^  (2.38) 

where  a*  is  the  accelerometer  bias,  w*  is  the  additive  white  Gaussian  noise,  and  f  *  is 

the  corrupted  specific  force  measurement,  resolved  in  the  b-frame. 

The  accelerometer  bias  will  be  modeled  as  a  first  order  Gauss-Markov  process, 
defined  below: 

a'’=-— a'’+w'  (2.39) 

^  ^hias  ^  ^ 

a 

where  is  the  time  constant  and  is  the  process  driving  white  noise. 

2.5.4  Gyroscope  Sensor  Model.  INS  systems  typically  incorporate  a  triad  of 
angular  rate  gyros,  aligned  with  each  of  the  three  b-frame  axes.  These  rate  gyros  measure 
the  angular  rotation  rate  of  the  body  frame  relative  to  inertial  space,  and  are  represented 
by  (oJO  [23]. 
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When  dealing  with  the  b-frame  relative  to  the  n-frame,  the  Euler  angles  represent 
roll,  pitch  and  yaw  respectively.  When  put  into  vector  form,  and  resolved  in  the  n-frame, 
the  result  is  the  attitude  state  vector  [23]; 


(2.40) 


The  rate  gyros  will  be  modeled  in  a  similar  manner  to  the  accelerometers.  The 
simplified  rate  gyro  model  used  herein  has  the  angular  rate  measurement  corrupted  by  a 
bias  and  an  additive  white  Gaussian  noise  [26]. 


<=co:;+b^+w: 


(2.41) 


where  b*  is  the  rate  gyro  bias,  w*  is  the  additive  white  Gaussian  noise,  and  is  the 

corrupted  angular  rate  measurement,  resolved  in  the  b-frame. 

The  rate  gyro  bias  will  also  be  modeled  as  a  first  order  Gauss-Markov  process: 


(2.42) 


where  is  the  time  constant  and  is  the  process  driving  white  noise  [26]. 

2.5.5  Strapdown  INS  mechanization.  The  system  dynamics  driving  inertial 


navigation  will  not  be  covered  in  depth  here.  A  full  derivation  of  the  system  dynamics 
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can  be  found  in  [23]  and  [26].  This  section  will  cover  the  mechanization  equations  used 
to  estimate  the  navigation  state  (based  on  the  inertial  measurements),  but  will  not  explain 
their  derivation. 


2.5.5. 1  Attitude  Mechanization.  Recall  Equation  (2.40).  The  Euler  angles 
describe  the  relative  orientation  of  two  frames.  The  Euler  angles  of  concern  to  an  INS 
are  those  relating  the  body  frame  to  the  navigation  frame  of  choice.  This  research  will 
focus  on  the  n-frame  for  navigation,  although  the  use  of  the  i-frame  and  e-frame  are 
perfectly  valid  choices.  One  dimensional  rotations  about  each  of  the  axis  are  defined  by 
[23]; 


Rotation  y/  about  the  z-axis,  Cj  = 


cos^i^  smy/  0 
-sin^  cos^i^  0 
0  0  1 


(2.43) 


cos^ 

0 

-sin^ 

Rotation  6  about  the  y-axis,  Cj  = 

0 

1 

0 

(2.44) 

sin^ 

0 

cos^ 

Rotation  (j)  about  the  x-axis,  C3  = 


1 

0 

0 


0  0 
cos^zi  sin^zi 
-sin^zi  cos^zi 


(2.45) 
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The  order  in  whieh  the  individual  Euler  angle  rotations  are  applied  does  effeet  the 
outeome.  Applying  the  3-2-1  eonvention  to  the  n-frame  to  b-frame  DCM  yields: 


c:=c,c,c, 

(2.46) 

(c;f  =c/cjc/ 

(2.47) 

C 


n 

b 


cos  6  cosy/ 
cosOsmy/ 
-sin0 


-  cos  ^  sin  ^  +  sin  (f)  sin  6  cos  y/ 
cos  (!>  cos  ^  +  sin  ^  sin  9  sin  y/ 
sin  (f)  cos  9 


sin  ^  sin  ^  +  cos  (f)  sin  9  cos  y/ 
-  sin  (j)  cos  y/  +  cos  (j)  sin  9  sin  y/ 
cos  cos  0 


(2.48) 


The  aetual  Euler  angles  are  not  used  in  the  INS  meehanization.  Only  the  DCM 
is  needed.  The  Euler  angles  ean  be  extraeted  from  in  order  to  display  attitude  to 
the  pilot,  but  this  will  be  diseussed  later. 

In  [23],  Titterton  and  Weston  apply  the  differential  Equation  (2.3)  to  and 
show  that 


where 


C«  _  p«o* 

b  - 


^nb=^nb^  = 


0 

ds^ 

dt 

ds„ 


de,  bSy 


dt 

0 


dt  dt 


dt 

dt 

0 


(2.49) 


(2.50) 
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where  is  the  skew  symmetrie  form  of  the  b-frame’s  angular  rate  relative  to  the  n- 
frame,  resolved  in  the  b-frame  («>*;,),  and  are  small  angles  about  the  3  axes.  For 
very  small  time  steps  {dt),  the  DCM  can  be  propagated  as  follows: 

Cl{t+5t)  =  Cim+^'nM  (2.51) 


The  measured  angular  rate  experienced  by  the  rate  gyros  ( )  is  the  sum  of  the 
body-to-nav  rate  (o)*^)  and  the  nav-to-inertial  rate  resolved  in  the  b-frame  (C*  (o'),), 
yielding: 


_n 


(2.52) 


The  angular  rate  of  the  n-frame  relative  to  the  i-frame  ( (o"„ ),  is  the  sum  of  the 
sidereal  angular  rate  ((o'),),  and  the  transport  rate  ((o”„),  caused  by  the  n-frame  moving 
about  the  Earth  [23]: 


+  h  Rj^  +  h  Rj^j  +  h 


(2.53) 


(0 1  =  (ol  +  (o: 


— — —  +  Q.  cos  L 
R^+h 


Rf^+h 


tan  L 

Rf^+h 


T 


Q.^  sin  L 


(2.54) 
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where  and  are  the  east  and  north  veloeities  of  the  n-frame  respeetively.  , 

h  and  L  are  the  previously  defined  Earth  radii  of  eurvature,  height  above  the  Earth,  and 
latitude  of  the  n-frame  respeetively. 

The  measured  b-frame  to  n-frame  angular  rate  (resolved  in  the  b-frame)  beeomes 
a  funetion  of  the  rate  gyro  measurements  (co|),),  and  the  ealeulated  sidereal  rotation 
veetor,  dependant  on  latitude  ,  from  Equation  (2.54))  [23]; 


—  b  —b  r^bjrn 

CO  L  =  COl— U  (0 

nb  lb  n  in 


(2.55) 


Combining  Equations  (2.49)  and  (2.55)  yields  the  differential  equation  relating 
the  DCM  of  interest  (C^),  with  the  measurements  reeeived  by  the  rate  gyros 


((o|],  =>  Ob’.f},  and  the  ealeulated  sidereal  rate  (a)"„  =>  Q”„): 


c:=c:q;;-q::c: 


(2.56) 


In  order  to  be  a  valid  DCM,  the  properties  deseribed  in  Seetion  2.3  must  be 
maintained  after  eaeh  propagation  eyele.  Unfortunately,  due  to  eomputational 

limitations,  the  eomputed  DCM  (C^)  rarely  has  an  orthonormal  basis,  nor  is  it  unitary. 

This  requires  an  ortho-normalization  after  eaeh  time  step.  Paehter  develops  an  algorithm 
for  this  task  [17].  Given  an  imperfeet  DCM  resulting  from  an  INS  propagation  proeess 

(C.J: 
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(2.57) 


Cl 

C2 

C3 

Wi 

Cji 

^22 

^23 
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W2 

_Ci 

C2 

C3  j 

_W3_ 

where  is  the  row  of  .  An  orthogonal  DCM  ean  be  ereated  ( C  ) 


C  =T  ,  C 

opt  ortn  err 


1  l  +  w/w, 
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w/w. 


w/Wj  +  wJWj 
w/Wj  + 


w/w^ 


w/Wj 


w/Wj  +  wJWj 

1  l  +  w/w^ 

2  wjwj 

wjw3^ 

WJW3+W3"W3 


w/W|  +  W3^W3 


wjw3 


wJW3+W3"W3 
1  1  +  W3’^W3 


2  w/w. 


(2.58) 


For  small  time  steps  ((5t),  (obtained  from  Equations  (2.54)  and  (2.55))  ean  be 

applied  to  Equation  (2.51)  in  skew  symmetrie  form.  The  result  is  made  orthogonal, 
produeing  the  attitude  DCM  propagation  equation  (valid  to  first  order).  It  is  a  funetion  of 
the  rate  gyro  measurements  and  the  navigation  state  estimate. 


orth 


c;(0+te-c 


(2.59) 


2. 5. 5.2  Position  Mechanization.  This  researeh  will  eoneentrate  on  the  n-frame 
resolution  of  the  navigation  state.  Reeall  that  the  n-frame  is  attaehed  to  the  body,  and  the 
axes  are  aligned  to  North,  East,  and  Down  (NED)  respeetively.  Eor  this  reason,  a 
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navigation  position  inside  the  n-frame  is  meaningless,  as  the  body  is  always  loeated  at  the 
origin.  Instantaneous  navigation  veloeities,  however,  ean  be  expressed  in  the  n-frame 
with  meaning.  For  this  reason,  the  navigation  position  states  (latitude,  longitude  and 
height  above  the  Earth),  are  external  to  the  n-frame.  They  are  obtained  by  integrating  the 


n-frame  veloeities  [23]. 
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(2.60) 
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(2.62) 


where  p  =  [E  /  is  the  position  vector  (latitude,  longitude  and  height  above 

respectively),  and  v"^  is  the  velocity  of  the  n-frame  relative  to  the  e-frame,  resolved  in 
the  n-frame.  and  are  the  previously  defined  Earth  radii  of  curvature.  The 
transformation  relates  the  velocity  ( v"^ ),  which  is  in  units  of  m/s,  to  the  change  in  the 


44 


position  state  (p),  which  is  in  units  of  radians  of  latitude,  longitude  and  meters.  The 
inverse  transform  is  also  used,  and  is  defined  as  follows: 

(2.63) 

2. 5. 5. 3  Velocity  Mechanization.  In  [23],  Titterton  and  Weston  develop  the 
navigation  equation  to  solve  for  the  velocity  as  a  function  of  the  accelerometer  triad 
measurements  (f^),  the  body-to-nav  DCM  {Cl),  the  sidereal  angular  rate  ((o'],),  the 

transport  rate  ((o"„ ),  and  local  gravity  (g" ),  all  described  in  previous  sections: 


v”  -(2(o" +(o"  )xv"  +g" 

en  b  lb  V  '^le  en/  en  w. 


(2.64) 


To  mechanize  this  relationship  with  measurements  and  calculated  values.  Equation  (2.64) 
becomes: 


v:„=c:f.:-(2«”+«:)xv:+g;  (2.65) 

For  small  time  steps,  Euler  integration  may  be  used  to  propagate  both  the  position 
and  velocity  states  as  follows: 


y(t+^t)  =  y(t)  +  ^{t)8t 


(2.66) 


45 


This  approximation  cannot  be  used  for  the  attitude  state  however.  Attitude  propagation 
requires  Equation  (2.51). 

2. 5. 5.4  Navigation  State.  The  Navigation  State  Vector  (x)  is  comprised  of 
the  9  modeled  states  of  interest,  that  have  been  deseribed  up  to  this  point:  position, 
veloeity  and  attitude  [23]. 


’  eb 


nb 


=  [L  I  h 


\  (j)  6  xfj'^ 


(2.67) 


The  Euler  attitude  angles  ('E”^)  are  extraeted  from  in  the  following  manner  [26]: 


r  C"  ^ 
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\^b33  y 
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^  nb 

e 

= 

aresin(-C;,,) 

( C"  ^ 
aretan  —5JL 

L  {Cln) 

(2.68) 


where  Cl^  is  the element  in  the row  of  . 

2.5.6  Error  States.  Typical  INS  applications  use  the  previously  described 
meehanization  to  generate  estimates  for  the  navigation  state  (x(t)).  Due  to  errors  in 
sensor  measurements  and  algorithmic  imperfections,  this  estimate  diverges  from  the  true 
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state.  Generally,  a  state  estimate  ean  be  modeled  as  the  sum  of  the  true  state  ( x(t)  )  and 


the  error  state  (6x(t) )  [23]. 


x(0  =  x(0  +  5x(0 


(2.69) 


In  the  ease  of  inertial  navigation,  the  relationship  between  error  states  and  whole 
states  is  more  complieated.  To  begin  with,  a  15-state  error  vector  (5x(t))  is  defined  by 
the  following  relationships: 


^x  = 


eb 


Kt 

a'’ 

b' 


(2.70) 


where  and  (5v"^  are  the  errors  in  position  and  velocity,  a*  and  b*  are  the 
accelerometer  and  gyro  biases  respectively,  and  e"^  is  a  vector  of  angular  errors  about 
each  of  the  three  n-frame  axes. 


««  r  i''' 

e  I  =  £  £  £ 

nb  L  y  ^  J 


(2.71) 


The  error  state  dynamics  can  be  expressed  as  a  linear,  stochastic  state-space 
model  driven  by  white  Gaussian  noise  [14]. 


5x(t)  =  F(t)5x(t)  +  G(0w(t) 


(2.72) 
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where  F(^)  and  G(^)  are  time  varying  funetions  of  the  whole  navigation  state,  and  w(t) 
is  the  dynamics  driving  noise  associated  with  the  inertial  sensors  and  their  biases. 

The  component  dynamics  have  already  been  developed: 

From  Equation  (2.60): 

P  =  T„X„  (2.73) 

From  Equation  (2.64): 

v:„=c:f;t-(2<+tD:jxv:„+g;  (2.74) 


From  Equation  (2.56): 


(2.75) 


From  Equation  (2.39): 


From  Equation  (2.42): 


a'’=-— a'  +  w5 


1 


b'’+w' 


(2.76) 


(2.77) 


The  errors  are  assumed  to  be  small.  Therefore,  small  perturbations  about  a 
nominal  may  be  used  to  analyze  the  error  dynamics  [17]. 


(2.78) 

dy 

(2.79) 

C>[I-(8x)]C: 

(2.80) 
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Substituting  these  state  error  definitions  into  Equations  (2.73)-(2.77),  produces  the 


desired  error  dynamics  equations. 


^p  =  T  ^v"  +^T  v” 

ne  en  ne  e 


(2.81) 


Through  some  manipulation; 
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(2.82) 
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Rg  COS  L 


(2.83) 


Jp  =  T  Sw"  +T  Jp 

ne  en  pp  * 


(2.84) 


Titterton  and  Weston  [25]  show  that: 


=[c:fxK,+cx-(2<+(o:jxjv:„-(2j<+j(o:jxv:„-jg:  (2.85) 


49 


Through  some  manipulation,  they  also  show: 


=G,^p+[G,  -2Q«  +[qf  xk, +c:a^ +c>:  (2.86) 


where 


f 


-V, 


1Q,„  eosT+- 


R^^  eos  L  j 


2X2.^  (v^  cos L-v^  sin T)  + 


R„  cos  L 


2Q.^Vj,  sin  L 


0 

0 


Ve  tanT-v^v^ 

R<" 

v^(v^tanT+v^) 

Ro" 

v^'+v/ 

R,^ 


(2.87) 


R„+h 


Rj^+h 


tan  L 

Rf^+h 

tanT+v^ 

R^^+h 


R^+h 


(2.88) 


The  matrices  Gj  and  Gj  account  for  the  errors  in  the  in  local  gravity  ((5g"),  due  to  the 
errors  in  position  and  velocity. 

Titterton  and  Weston  [25]  develop  the  final  dynamics  equation  needed  to  express 
the  error  in  attitude,  resolved  in  the  n-frame: 

8  =  -Q”8-C:b^-CX^_^^+5(o”  (2.89) 


50 


where 


„  „  5(o”  8(0"  5. 

5co"  = — ^5P  +  — ^5v  + 

5P  5v 


5®" 

^in 

58 


58  «  0 


(2.90) 


5®"  =E  5P  +  E,5v 


o  n 

®”,  is  not  a  funetion  of  8 ,  allowing  the  term  Sg  to  be  dropped.  Taking  ro"^  from 
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Equation  (2.54): 


-Q.^  sin  L 
0 


-Q.  eos  L- 


(Re+I') 

(R„  +  hf 
VjE  tan  L 


{Rj,+h)cos^L  {Rf,  +  h) 


(2.91) 


0 


1 


1 


(^Rj^+h) 

0 


[Rj,+h) 

0 

tanE 

[Rn+H) 


(2.92) 


To  satisfy  the  dynamies  model  from  Equation  (2.72),  the  driving  noise  veetor 
( w(t)  )  must  inelude  the  sensor  measurement  and  bias  driving  noises.  It  is  defined  as: 


w(0  = 


w 


b 

“hia.. 


(2.93) 
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Combining  Equations  (2.84),  (2.86),  (2.91),  (2.76)  and  (2.77),  the  overall  error 
dynamies  equation  is  obtained  for  the  n-frame. 
d^{t)  =  ?[f,  x(0]  •  SyL{t)  +  G(0  •  w(0 


()X(0  = 


■  pp 


0, 


0, 


[G: 


^  ne 

E. 

O3 

0. 


O3 

-Q” 


0, 


0, 


0. 


0. 


—  L 


0, 


O3 

0 


3 

c: 


0. 


— L 


()X(0 


15x]5 


+ 


0,  0,  0,  0, 


0, 


03  03  03 

-c:  0,  0, 


O3  O3  I3  O3 

0,  0,  0,  L 


w(0 


J5x]2 


(2.94) 


In  many  eases,  the  Earth  ean  be  modeled  as  flat  and  non-rotating.  This 
approximation  is  aeeeptable  if  the  rate  gyros  eannot  piek  up  the  Earth’s  rotation  and  if  the 
navigation  is  eonstrained  to  a  relatively  small  area.  This  approximation  allows  many 
terms  to  be  dropped,  simplifying  Equation  (2.94)  and  redueing  the  eomputational  load. 
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To  model  the  Earth  as  flat  and  non-rotating,  &  0  and  oo  .  This 


reduces  Equation  (2.94)  to: 

■O3 

^ne 

O3 

03 

O3  “ 

O3 

O3 

0 

X 

01 

O3 

■O3 

03 

03 

03“ 

O3 

O3 

03 

03 

-01 

01 

03 

03 

03 

(5x(0  = 

O3 

O3 

03 

--I3 

03 

k 

-f 

03 

03 

-c: 

03 

03 

I3 

0  0 

w(0 

O3 

O3 

03 

O3 

--I3 

.03 

03 

03 

l3_ 

15x12 

- 

15x15 

(2.95) 

Note:  eannot  be  modeled  to  have  Rg=  Rj^=Rj,  ^oo  beeause  it  is  needed  to  eonvert 

veloeity  units  into  degrees  of  latitude  and  longitude.  This  transform  remains  in  the 
simplified  error  equation. 

The  basis  for  INS  meehanization  and  the  assoeiated  error  model  have  been  laid. 
The  first  nine  error  states  are  the  well  understood  Pinson  error  states  [27],  allowing  a 
feedbaek  extended  Kalman  filter  to  be  used  for  aiding.  The  Kalman  filter  used  to  aid  the 
INS,  ean  now  be  discussed. 
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2. 6  Kalman  Filtering 

Kalman  filtering  is  the  backbone  of  INS  aiding.  It  provides  an  optimal  estimate 
of  the  desired  state  vector,  based  on  imperfect  measurements  that  indirectly  reflect  the 
true  state  values.  An  extended  Kalman  filter  is  used  herein  to  aid  the  INS  through 
bearings-only  measurements.  A  development  of  the  EKF  follows  to  aid  understanding  of 
the  eventual  algorithm.  This  section  presents  the  stochastic  modeling  and  Kalman 
filtering  concepts  outlined  by  Maybeck  [14] [15]. 

2.6.1  Linear  Stochastic  System  Model.  The  linear  stochastic  system  model  is 
essentially  the  linear  system  model  (Equation  (2.5))  with  an  additional  stochastic  term  to 
account  for  the  uncertainty  of  the  model  dynamics.  The  general  form  of  the  linear 
stochastic  system  model  can  be  written  in  the  form  of  a  linear  stochastic  differential 
equation  [14]. 

x  =  Fx  +  Bu  +  Gw  (2.96) 

where  x  is  again  the  state  vector,  but  it  is  now  a  vector  of  random  variables.  U  is  the 
deterministic  input,  F  and  B  are  the  system  dynamics  and  input  matrices  respectively. 
G  is  the  noise  transformation  matrix,  and  w  is  the  vector  of  white,  Gaussian,  dynamics 
driving  noises,  w  is  a  zero-mean  Gaussian  process  with  a  covariance  kemal,  defined  by: 

E  { w(0w^  (t+r)}  =  Q{t)8{x)  (2.97) 
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where  Q(^)  is  the  noise  strength  matrix,  and  d{x)  is  the  Dirae  delta  funetion. 

The  solution  to  stoehastie  differential  Equation  (2.96)  is  a  Gaussian  random 
veetor  itself  The  dynamies  driving  noise  ereates  uneertainty  in  the  solution  to  an 
otherwise  trivial  problem.  The  solution  is  defined  by  its  mean  and  eovariance  [6]. 

The  mean  of  the  random  veetor  x(t) ,  for  t  e  [^^,00)  is  defined  as  follows: 


m,(0  =  E{x(0}  =  0(a«)E{x(t,)}  +  jo(u)B(T)u(T)aT  (2.98) 


with  the  initial  eondition 

E{x(t„)}=x,  (2.99) 

Note  that  is  derived  in  the  same  manner  as  in  Seetion  2.4.1  and  Equation  (2.7). 

The  addition  of  the  driving  noise  has  no  effeet  on  O(tTo)  the  mean,  whieh  is 
equivalent  to  the  deterministie  solution  from  Equation  (2.6)  [14]. 

The  eovarianee  of  the  solution  is  defined  to  be 


P,,  (0  =  E  {x(0x^  (0}  -  m,  {t)m,  (tY  (2. 1 00) 


and  is  found  by  solving 


p,,(0  =  O(a«)p,,(to)o^(ao)+ jo(t.T)G(T)Q(T)G"(T)o"(t.T)aT  (2.101) 


with  the  initial  eondition 


^xx(to)  “  Pq 


(2.102) 
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Due  to  the  Gaussian  nature  of  the  solution,  the  eomputed  mean  provides  the  best 
estimate  for  the  true  state: 

x(0  =  m^(0  (2.103) 

The  eomputed  eovariance  deseribes  the  uneertainty  of  the  estimate.  The  subseript  will  be 
dropped  heneeforth. 

P(0  =  Pxx(0  (2.104) 

2.6.2  Linear  Measurement  Model.  In  order  for  a  Kalman  filter  to  be  of  use, 
measurements  of  the  states  must  be  taken.  Due  to  their  very  nature,  measurements  are 
often  discrete.  The  continuous  measurement  case  will  not  be  covered.  In  the  linear 
stochastic  case,  measurements  of  the  states  are  corrupted  by  additive,  white  Gaussian 
noise  [14]. 

z(t,)  =  H(t,)x(t,)  +  v(0  (2.105) 

where  z(t.)  is  a  vector  of  measurements  taken  at  a  specific  time  instance  {t  =  t,  ) .  H(t.) 
is  the  observation  matrix,  relating  the  states  to  the  measurements  produced  by  the  sensor, 
and  v(t.)  is  the  sensor  corruption  noise  vector,  with  covariance  kernel  defined  as: 


e{v((,)v'(<,))  =  R(<,)<S„  (2.106) 

where  R(t,.)is  the  covariance  matrix  for  the  corruption  noise,  and  is  the  Kroeneker 
delta  function. 
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2.6.3  Linear  Kalman  Filter.  The  Kalman  filter  allows  the  use  of  imperfeet 
measurements,  to  estimate  the  solution  to  the  stoehastie  differential  Equation  (2.96). 
Without  measurements,  the  eovarianee  of  the  solution  would  grow  unbounded,  rendering 
the  state  estimate  meaningless  to  a  user.  Kalman  filtering  bounds  the  eovarianee,  or 
uneertainty  in  the  estimate,  but  does  not  provide  perfeet  knowledge.  In  the  linear  ease, 
the  Gauss-Markov  nature  of  the  state  model  allows  the  use  a  simple  reeursive  eyele, 
repeating  for  every  measurement. 

2. 6. 3.1  Kalman  Filter  Cycle.  The  Kalman  filter  eyele  has  two  parts:  the 
propagation  phase,  and  the  update  step.  In  the  propagation  phase,  the  Kalman  filter 
estimates  the  new  mean  and  eovarianee  of  the  state  estimate  for  some  time  passage. 
During  the  update  step,  the  Kalman  filter  adjusts  the  propagated  mean  and  eovarianee 
based  on  measurements  taken  in.  This  eyele  is  repeated  for  every  measurement.  Figure 
2.6  illustrates  this  eyele  [6][14]. 


Measurement  taken  at  t. 


Propagation  phase  moves  estimate  forward  in  time,  I 

1  1  +\  r%/^  +\  t  .  t  ^ 


Update  step  generates  new 
estimate  at  t3 


Figure  2.6:  Kalman  filter  cycle. 
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Note  the  eonvention  used  to  indieate  time  immediately  before  and  after  a 
measurement  (illustrated  in  Figure  2.6,  and  explained  below): 

•  t-  refers  to  an  instanee  in  time;  typieally  when  a  measurement  is  taken. 

•  tr  refers  to  the  very  instant  before  .  x(t.  j  )  is  the  Kalman  filter  estimate  the 

instant  the  measurement  is  taken,  but  just  before  it  is  applied.  x(t.  j  )  and 
P(t.  j  )  are  the  output  of  the  propagation  phase. 

•  refers  to  the  very  instant  after  a  measurement  is  taken,  and  the  update  step  is 
applied.  x(6_i^)  and  P(6.i^)  are  the  output  of  the  update  step. 

2.63.2  Propagation  Equations.  The  state  estimate  and  eovarianee  are 
propagated  through  time  using  the  solution  to  the  stoehastie  differential  Equation  (2.96) 
shown  in  Equations  (2.98)  and  (2.101)  [14].  Applying  the  Kalman  filter  eyele 
eonvention,  the  propagation  phase  beeomes: 

x(t,-)  =  0(t,.,t,Jx(t,/)+|0(t,,T)B(T)u(T)aT  (2.107) 

tj 

P(t7)  =  0(t,t,JP(t,/)0\t,T,J+j(h(t,,T)G(T)Q(T)G"(T)(h\t,.^  (2.108) 

In  the  diserete  time  ease: 


x(t,-)  =  0(t,T,,i)x(t,/)  +  B,(t,i)u(t.  i) 


P(tr )  =  0(t,  T, JP(t,/ )0^  (t,.  T,  J  +  G,  (6  JQ,  (t, JG/ 


(2.109) 

(2.110) 
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where  u(0  is  assumed  eonstant  over  the  interval  t  e  ,  and  ; 


(2.111) 

tj 

G,(t,JQ,(t,,)G/(t,J  =  1 0(t,.T)G(T)Q(T)G"(T)(h"(t,T)eT  (2.112) 

^i-\ 

2.63.3  Update  Equations.  During  the  update  step,  the  state  estimate  and 
eovarianee  are  adjusted  to  aeeount  for  the  measurement  eoming  in.  The  Kalman  filter 
gain  appropriately  weighs  the  measurement  against  the  estimate,  as  a  funetion  of  the 
system  model  [14]. 

Kalman  filter  gain; 

K(<,)  =  P(f,-)H"(f,)[H(<,)P(<r)HV<,)  +  Rft)]"'  (2.113) 

Update  Equations; 

x(t2 )  =  x(t.- )  +  K(t. )  [z(t. )  -  H(t,  )x(tr )]  (2.114) 

P(t2)  =  P(t,-)  -K(OH(t,)P(t,-)  (2.1 15) 

2.6.4  Non-Linear  Stochastic  System  Model.  In  many  oases,  a  linear  system 
model  does  not  suffioe.  The  non-linear  stoohastio  system  model,  used  in  this  researoh,  is 
a  variation  on  the  deterministio  non-linear  system  from  Equation  (2.13).  Eike  the  linear 
model,  it  has  an  additional,  dynamios  driving,  white  Gaussian  noise  term  [15]. 

x(0  =  f  [x(0,  u(0,  t]  +  G(0w(0  (2.116) 
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where  is  defined  as  in  the  linear  case;  Equations  (2.96),  (2.97). 

2.6.5  Non-Linear  Measurement  Model.  The  non-linear  measurement  model 
takes  on  a  similar  form  [15]. 


z(t,)  =  h[x(t,),t,]  +  v(t,)  (2.117) 

where  h[x(t^),tj  is  a  non-linear  function  of  x(t.)  and  time,  and  v(t.)  is  defined  as  in  the 
linear  case;  Equation  (2.105). 

2.6.6  Linearized  Model.  The  non-linear  stochastic  system  model  massively 
complicates  the  estimation  process.  The  solution  to  the  stochastic  differential  is  no 
longer  trivial,  and  in  some  cases,  unsolvable.  fortunately,  many  non-linear  systems  can 
be  modeled  as  linear  for  small  perturbations  about  a  nominal  trajectory.  The  extended 
Kalman  filter  takes  advantage  of  this  property. 

Because  the  dynamics  driving  noise  is  additive,  it  does  not  complicate  the 
linearization  process.  The  non-linear  stochastic  system  may  be  approximated  to  be  linear 
about  a  nominal  trajectory  as  described  in  Section  2.4.2  [15]. 

x(t)  +  6x(t)  =  f  [x(0,  u(0,  t]  +  F[t;  x(t),  u(0]5x(t)  +  G(0w(t)  (2.118) 

5x(0  =  F[t;  x(0,  u(0]6x(0  +  G(0w(0  (2.119) 


60 


F[t;x(0] 


af[x,u(0,^] 

0X 


x=x(0,u(0,« 


•• 

dx„ 

dxj 

1 

s: 

(2.120) 


The  resultant  system  models  the  small  perturbations,  or  errors  about  the  nominal. 
This  fits  well  with  the  error  states  ehosen  for  the  INS.  Note  that  the  nominal  ehosen  is 
the  state  estimate  ( x(t) ).  The  optimal  nominal  ehoiee  would  be  the  true  state,  but  sinee 
the  true  state  is  unobtainable,  x(t)  provides  a  reasonable  approximation  [15].  The  result 
is  a  poorer  performing  Kalman  filter.  Additionally,  the  input  vector  is  assumed  to  be 
known  (5u(t)  =  0). 

The  same  process  can  be  applied  to  the  measurement  equation  [26][15]. 


)  =  h[x(t.,i- ),  )]5x(t.,i )  +  ) 


(2.121) 


5z(t,,i)  =  H[t,,j;x(t.,i)]5x(t.,i)  + v(t.,i) 


(2.122) 


H[t,;x(tr)] 


gh[x,t,] 

5x 


dh, 

dh, 

dXj 

dXn 

II 

aT 

II 

X 

dh 

m 

dK 

dXj 

1 

s: 

(2.123) 
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2.6.7  Extended  Kalman  Filter.  After  linearization,  the  system  error  model  fits 
the  linear  form  needed  to  create  a  linear  Kalman  filter.  Maybeck  develops  the  extended 
Kalman  filter  (EKF)  fully,  yielding  the  propagation  and  update  equations  as  a  function  of 
the  linearization  gradients  (F[t;x(t),u(0]and  H[t.;x(tr)])  [15]. 

2. 6. 7.1  EKF  Propagation  Equations.  The  EKF  estimate  is  propagated 
forward  to  the  next  time  sample  t^^^  by  integrating  the  following  over  the  interval 

i(t/t,)  =  f[x(t/t,),u(0,t]  (2.124) 

P{t/t.)  =  F[t;x(t/t.),u(0]P(t/t,)  +  P(t/t,)F^[fix(t/t.),u(0]  +  G(0Q(0G^0  (2.125) 
with  the  initial  conditions; 

x(t,/t,)  =  x(tf)  (2.126) 

P(t/t,)  =  P(t/)  (2.127) 

The  propagation  produces: 

x(t,,r)  =  x(t,,/t,)  (2.128) 

P(W)  =  P(^m/0  (2.129) 

2. 6. 7. 2  EKF  Update  Equations.  The  EKF  update  is  accomplished  by  the 
following  equations  [15]; 

K(t,)  =  P(tr)H"[t,;x(tr)]{H[t,;x(tr)]P(t,.-)H^[t,;x  (2.130) 
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dx{t; )  =  dx{t- )  +  K{t, )  {z(^ . )  -  h[x(^,r ),  ]  -  H[^. ;  x(^,r  )]^x(^. )} 


(2.131) 


Pa;)  =  P(^,  )-K(OH[^,;x(^,-)]P(^r)  (2.132) 

where  (5x(^.)  is  the  error  state  vector  containing  the  Pinson  errors  states. 

2.6.8  Extended  Kalman  Filter  and  INS  Integration.  Figure  2.7  illustrates  how 
the  EKF  and  INS  work  together  to  bound  the  error  growth. 
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Figure  2,7:  Extended  Kalman  Filter  and  INS  integration.  The  EKF  receives  the 
state  estimate  from  the  INS  system  dynamics,  as  well  as  measurements  from  the 
update  sensors.  The  EKF  produces  an  error  state  estimate,  and  applies  it  as  a 
correction  to  the  INS  [26], 
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2.6.9  Implicit  Measurement  Equation.  Sometimes,  the  measurement  model 
used  does  not  fit  the  eonvenient  form  in  Equation  (2.117).  Non-linearities,  or  singular 
matrices  may  not  allow  one  to  isolate  the  measurement  vector  on  the  left  hand  side  of  the 
equation.  In  this  case,  the  implicit  measurement  model  may  be  used  [19]. 

0  =  g(x,y)  (2.133) 

z  =  y  +  v,  v~N(0,R)  (2.134) 

(y-z)  +  H.O.T.  (2.135) 

where  g(x,y)  is  the  functional  relationship  between  the  true  state  x  and  the  perfect 
measurement  y  .  A  new  measurement  model  can  be  created; 


g(x,y)«g(x,z)  +  ^ 

0X 


^  5g 
^x  +  — 

5y 


g(x,z) 


^x  +  v'. 


v'~  A(0,R') 


(2.136) 


R'  = 

5 

R 

[ay 

[ay 

zy 

(2.137) 
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where  g(x,z)  is  substituted  for  z  in  the  EKF  update  Equation  (2.131),  and  R'  is 
substituted  for  Rin  the  EKE  gain  Equation  (2.130).  Easily,  H[t.;x(t,.  )]  is  replaeed 


with 


3g 


5x 


The  rest  of  the  EKE  remains  unehanged. 


The  extended  Kalman  filter  will  be  used  to  aid  the  INS  using  bearings  only 
measurements  from  imagery.  The  opties  involved  with  taking  images  ean  now  be 
diseussed. 


2. 7  Imaging 

In  order  to  aid  INS  with  imagery,  the  mathematies  of  opties  must  be  understood. 
This  seetion  presents  the  aspeets  of  this  broad  field  whieh  will  be  applied  to  the  researeh 
herein,  to  inelude  optieal  projeetion  theory,  sensor  models,  measurement  equations,  and 
feature  traeking. 

2.7.1  Optical  Projection  Theory.  Optieal  projeetion  theory  relates  the  real 
world  geometry  to  the  geometry  of  an  image  passed  through  a  lens.  The  thin  lens  model 
relates  the  image  to  the  true  seene  aeeording  to  Eigure  2.8,  and  the  following  equation 

[10]: 


i  +  i  =  (2.138) 

Z  z  / 


where  Z  and  z  are  the  distanees  of  the  objeet  and  image  to  the  lens  respeetively,  and  /  is 
the  foeal  length  of  the  thin  lens. 
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Figure  2,8:  Thin  lens  camera  model.  The  thin  lens  model  directs  parallel  light  rays 
toward  the  focus,  resulting  in  an  image.  Figure  is  not  to  scale  [26]. 


Decreasing  the  aperture  of  the  thin  lens  to  a  theoretical  zero  allows  the  use  of  a 
pinhole  camera  model  [10].  The  pinhole  camera  model  relates  the  size  of  an  image,  as  it 
falls  onto  an  image  plane,  to  the  size  of  the  true  object,  as  given  in  Equation  (2.138)  and 
illustrated  in  Figure  2.9.  In  order  for  a  pinhole  model  to  be  applied,  the  “fish  eye” 
distortion  created  by  a  wide  angle  lens  must  be  removed  from  the  image  and  associated 
features.  A  discussion  of  how  this  is  accomplished  is  included  in  [36].  For  the  purposes 
of  this  discussion,  fish  eye  distortion  will  be  assumed  to  be  removed  from  all  images  and 
the  pinhole  camera  model  assumed  valid. 
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SCENE  ^ -  / 


IMAGE 

Figure  2,9:  Pinhole  camera  model.  The  pinhole  camera  is  a  theoretical  camera 
model  in  which  a  thin  lens  aperture  approaches  zero.  The  projected  image  is 
inverted  on  the  image  plane  [26], 


Inverting  this  image  has  the  effect  of  righting  it,  and  projecting  it  one  focal  length 
in  front  of  the  aperture;  see  Figure  2.10.  Consider  a  point  source  object.  The  projection 
of  the  object’s  image  onto  the  image  plane  )  is  a  function  of  the  object’s  position 
vector,  resolved  in  the  c-frame  ( S'’ ),  and  the  focal  length  of  the  camera  lens  [10]. 


(2.139) 


where  $1  is  the  z-component  of  s'’ ,  or  distance  of  the  point  source  from  the  aperture. 


projected  onto  the  c-frame ’s  z-axis. 


Figure  2.10:  Image  projection  model.  A  point  source  object  (s^)  is  projected  onto 
the  image  plane  of  the  camera  (s^™^ )  [26]. 


Imaging  arrays  lie  on  the  image  plane  in  order  to  capture  the  focused  image. 
They  are  made  up  of  a  (  M  x  A  )  array  of  rectangular  pixels.  The  upper  left  pixel  has  the 
coordinates  (1,1).  The  pixel  co-ordinate  frame  increases  down  and  right  [26]  (see  Figure 
2.11). 
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Figure  2,11:  Camera  image  array.  The  imaging  array  of  the  camera  consists  of  an 
(M  X  N)  array  of  pixels.  The  physical  height  and  width  of  the  array  are  represented 
hy  H  and  W,  respectively  [26], 

For  an  array  with  physical  dimensions  (HxW),  the  transform  to  obtain  pixel 
eoordinates  from  the  projeeted  image  veetor  is  [10]; 


(2.140) 


Note  that  S^“  is  a  two-dimensional  veetor  of  pixel  loeations.  It  does  not  have  to  take  on 
integer  values,  however. 
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Combining  Equations  (2.139)  and  (2.140)  yields  a  transform  from  the  eamera 


frame  to  the  pixel  plane  [26]. 
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(2.141) 


(2.142) 


Note  that  z^is  the  homogeneous  pixel  eoordinate  veetor  form  of  S^“.  It  is  also  the 
measurement  produeed  by  the  eamera. 

is  the  invertible,  homogeneous  transform  matrix  between  the  eamera  frame 
and  the  pixel  plane.  The  inverse  is  defined: 
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The  object’s  camera-frame  position  vector  is  thus  expressed  as  the  pixel  plane 
measurement,  transformed  into  the  c-frame,  and  scaled  by  the  z-component.  Given  the 
pixel  plane  measurement  z^,  one  must  estimate  the  z-component  of  the  object  position 

vector  in  order  to  estimate  the  full  object  position  vector.  This  can  be  done  many  ways, 
and  will  be  discussed  in  Chapter  3. 

The  goal  of  imaging  is  to  determine  the  relative  position  of  an  object  to  the 
camera  in  the  n-frame  (given  the  pixel  plane  vector).  Define  A^'^^to  be  the  relative 
difference  between  the  imaged/tracked  object  and  the  n-frame  origin  (resolved  in  the  n- 
frame).  Define  P^^^to  be  the  relative  position  of  the  camera  origin  (resolved  in  the  b- 

frame).  Define  C*  to  be  the  DCM  corresponding  to  the  camera  alignment  to  the  aircraft 
body.  Then  A"^,can  be  related  to  by  the  following  (see  Figure  2.12). 

a;=c:pl+c:c:s^  (2.145) 

a;=c:pl+5:c:t;,z^  (2.i46) 

Note  that  is  a  constant  matrix  expression  for  a  rigidly  mounted  camera,  and 
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Figure  2,12:  Relative  landmark  geometry.  The  n-frame  and  b-frame  share  an 
origin,  hut  are  aligned  according  to  the  aircraft  Euler  Angles  (Cl).  The  relative 

position  of  the  tracked  object  to  the  n-frame  is  the  sum  of  the  c-frame  object 
position  vector  (s")  and  the  camera  position  inside  the  aircraft,  resolved  in  the  n- 
frame(Clplj. 


Note  that  is  a  relative  veetor  position  in  the  n-frame  with  units  of  meters.  A”^, 

expressed  as  a  function  of  the  aircraft  and  target  geodetic  position  (latitude,  longitude, 
and  height)  is: 


A"  =T  (p  -p) 

‘-^tgt  'enyrtgl  r ) 


(2.147) 


where  p  and  p^^,  are  the  geodetic  positions  of  the  aircraft  and  target  respectively. 
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Substituting  Equation  (2.147)  into  (2.146)  yields  the  measurement  equation  for 
this  eamera  model; 

Z,  =  irrc;  (t,.  (p„  -p)  -  cipl, )  (i.m) 

2.7.2  Feature  Tracking.  Aiding  INS  with  imagery  ean  only  be  aeeomplished  if 
fixed  landmarks  on  the  ground  ean  be  traeked  from  image  frame  to  image  frame.  Many 
feature  traeking  algorithms  are  deseribed  by  Veth  and  others  [10]  [26].  The  inner 
workings  of  these  feature  traekers  will  not  be  diseussed  here,  merely  the  eharaeterization 
of  the  measurements  they  provide. 

For  the  purpose  of  this  discussion,  landmarks  fall  into  two  categories:  surveyed 
landmarks,  and  landmarks  of  opportunity  (LOO).  Surveyed  landmarks  are  those  with 
geodetic  positions  that  are  known  via  some  independent  source  and  can  be  identified  in  a 
scene.  Landmarks  of  opportunity  have  not  been  surveyed  independently  and  must  be 
located  as  they  are  tracked.  For  the  purpose  of  this  discussion,  a  feature  is  defined  as  the 
projection  of  a  specific  landmark  onto  the  image.  Feature  uniqueness  allows  successive 
matches  and  tracking  frame  to  frame. 

Feature  trackers  aim  to  track  a  landmark  from  frame  to  frame.  These  landmarks 
appear  as  a  collection  of  pixels  that  correlate  spatially  and  geometrically  between  frames. 
The  output  of  a  feature  tracker  is  the  homogeneous  pixel  coordinate  vector  .  Ideally, 

this  vector  would  correspond  to  the  exact  same  n-frame  location  with  every  frame; 
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whether  it  be  the  eenter  of  mass,  or  a  distinet  eomer,  is  moot.  Unfortunately,  this  pixel 
eoordinate  veetor  is  plagued  by  errors. 

The  first  major  error  is  due  to  the  quantization  nature  of  an  imaging  array  [26]. 
Pixels  are  diserete,  but  feature  loeations  aren’t  neeessarily.  Some  feature  generation 
algorithms  elaim  sub-pixel  level  preeision.  This  is  questionable  beeause  sueh  preeision  is 
beyond  the  spatial  sampling  frequeney  of  the  eamera.  While  the  feature  traeker  may 
interpolate  between  groups  of  pixels,  the  pixels  between  whieh  they  interpolate  are 
diserete.  This  induees  a  uniformly  distributed  spatial  error,  one  pixel  wide. 

Ip=Zp+n,  (2.149) 


where  is  the  veetor  form  of  the  uniform  random  variable  measurement  noise,  defined 
as  follows.  Note  that  the  third  element  is  always  zero,  as  it  is  a  two  dimensional  error; 


E{n,}  =  [0  0  of 


(2.150) 
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Additional  noise  sources  sum  together  and  are  modeled  as  additive,  white,  and 


Gaussian  in  nature. 


(2.152) 


where  is  the  vector  form  of  the  Gaussian  random  variable  measurement  noise, 
defined  as: 


E{n^}  =  [0  0  of 


(2.153) 
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(2.154) 


where  Rg  is  the  covariance  matrix  of  the  white  Gaussian  noise.  If  Rg  is  much  larger 
than  R^,  then  they  can  be  modeled  as  a  single,  white  Gaussian  noise,  but  this  is  a 
function  of  the  quality  of  the  tracking  algorithm  and  imaging  array  used. 


2. 8  Geometric  Dilution  of  Precision 

Non-linear  measurements  are  often  functions  of  geometry.  This  is  especially  true 
in  the  case  of  tracking  features  in  successive  images.  Some  geometries  are  more  suited  to 
deliver  better  performance  than  others  (e.g.,  as  tangents  approach  90  degrees, 
singularities  form).  A  quantifiable  measure  of  quality  (as  a  function  of  geometry)  is 
called  the  geometric  dilution  of  precision  (GDOP).  This  scalar  value  relates  the  relative 
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precision  or  quality  of  measurement  that  can  be  obtained  from  differing  geometric 
scenarios.  The  absolute  value  of  GDOP  is  less  insightful  than  the  difference  between  two 
GDOP  values;  a  high  GDOP  means  much  dilution,  whereas  a  low  GDOP  means  very 
little  dilution  [21], 

The  in  the  case  of  the  EKF,  GDOP  is  a  function  of  the  linearized  measurement 
observation  matrix  (H[t.;x(t.  )])  [19]. 

GDOP  =  ^tr{(H"[t,;x(t,-)]H[t,;x(t, -)])-'}  (2.155) 

High  GDOP  values  indicate  that  the  imprecision  of  imperfect  measurements  will 
be  magnified  by  geometric  effects.  Fortunately,  this  knowledge  is  built  into  the  EKF,  and 
the  relative  weighting  of  a  measurement  experiencing  high  GDOP  is  lowered.  This  effect 
causes  the  EKF  to  behave  much  like  it  does  with  a  very  uncertain  measurement  (large 
R(t.))  [19].  For  this  reason,  analyzing  the  GDOP  of  a  system  can  provide  insight  into 

the  performance  of  the  associated  EKF.  High  GDOP  essentially  weakens  the  aiding 
strength  of  a  measurement. 
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2.9  System  Observability 

A  linear  system  is  said  to  be  observable  if  knowledge  of  the  initial  state  ean  be 
learned  after  a  finite  amount  of  time  and  a  finite  number  of  measurements.  This  is 
signifieant  to  aiding  an  INS  beeause  learning  the  state  through  measurements  is  the  entire 
purpose.  For  the  time  invariant  ease,  the  observability  matrix  is  a  funetion  of  the  system 
dynamies  matrix  (F)  and  the  measurement  matrix  (H)  [26];  defined; 


H 

HF 

hf«i 


(2.156) 


where  n  is  the  dimension  of  the  assoeiated  state  veetor.  The  H  F  pair  is  said  to  be  fully 
observable  if  Q  has  rank  n.  This  means  that  the  measurement  model  ehosen  is  suffieient 
to  observe  the  total  state.  Beeause  both  matriees  are  funetions  of  the  state  veetor,  insight 
about  observability  ean  only  be  obtained  for  perturbations  about  the  given  state 
trajeetory.  In  the  ease  of  INS,  one  must  examine  Q  for  all  the  desired  flight  profiles  and 
envelopes;  no  sweeping  generalizations  may  be  made. 

In  the  time-varying  ease,  another  method  of  examining  the  strength  of  aiding 
aetion  (provided  to  the  INS  by  update  measurements)  must  be  used.  It  involves  studying 
the  observability  grammian.  A  system  is  said  to  be  eompletely  observable  if  the 
observability  grammian  (W(t)),  for  a  partieular  measurement  equation,  has  full  rank. 
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The  observability  grammian  is  a  funetion  of  the  system  dynamics  matrix  (F),  the 
observation  matrix  (H)  and  the  nominal  trajectory  about  with  the  system  is  linearized 

(xj. 

W  (<)  =  I  .  H"[x.  (f)]H[x.  {t)V‘dt  (2.157) 

0 

The  condition  number  of  W  (t)  also  speaks  to  the  observability  of  the  measurements.  A 

high  condition  number  indicates  weak  aiding  along  certain  trajectories  while  strong 
aiding  along  others. 

Another  method  of  expressing  GDOP  is  with  the  observability  grammian.  The 
GDOP  is  defined  as; 

GDOP  =  ^trace{w(t)^‘|  (2.158) 
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2.10  Summary 

In  summary,  this  chapter  laid  out  the  framework  to  design  an  INS  and  develop  an 
aiding  strategy  built  around  the  extended  Kalman  filter.  The  basie  opties  and  eamera 
referenee  frame  transforms  were  explored  to  allow  the  development  of  the  measurement 
equation  in  Chapter  3.  Lastly,  the  tools  by  whieh  the  performanee  will  be  predieted  and 
evaluated  were  presented.  Chapter  3  will  speak  to  the  speeifie  design  used  for  the  flight 
test. 
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III.  Developing  the  SLAAMR  Algorithm 


The  concept  of  simultaneously  mapping  an  environment  using  imagery  and 
navigating  using  this  map  is  not  new.  The  Simultaneous  Location  And  Mapping  (SLAM) 
process  has  been  proposed  and  discussed  in  many  studies  and  research  projects 
[1][4][5][8][11][19][23][24][26][28].  This  paper  adopts  the  fundamental  SLAM  process 
to  create  an  algorithm  that  can  simultaneously  locate  landmarks,  map  an  environment, 
and  aid  an  inertial  navigation  system,  specifically  using  a  recursive  process.  This  single 
routine  will  seamlessly  locate  and  map  landmarks  when  accurate  navigation  state  data  is 
provided  (i.e.,  GPS),  accurately  aid  the  Inertial  Navigation  System  (INS)  when  surveyed 
landmarks  are  available,  or  provide  both  tasks  in  the  absence  of  surveyed  landmarks  or 
GPS.  The  theory  behind  a  SLAM -based  aiding  strategy  is  well  understood,  and  has 
proven  to  perform  well  in  simulation.  However,  real  world  conditions,  image  processing, 
and  sensors  complicate  the  matter,  requiring  further  study  and  design  to  produce  a 
practical  SLAM-based  navigation  system.  This  chapter  explores  the  mathematics  behind 
the  problem  and  develops  the  design  of  a  low-cost,  monocular  optical-inertial  navigation 
system.  The  Simultaneous  Location  Aiding  And  Mapping  Recursively  (SLAAMR) 
algorithm  (proposed  herein)  provides  a  robust  design  to  accomplish  the  aforementioned 
tasks  seamlessly  in  the  airborne  environment. 

The  SLAAMR  algorithm  is  designed  to  accomplish  practical  INS  aiding,  using 
passive  and  low-cost  measures,  such  as  monocular  imagery.  It  is  specifically  designed 
for  use  with  a  low-cost,  poor  quality,  inertial  measurement  unit  (IMU)  aboard  an  airborne 
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platform.  The  use  of  a  reeursive  proeess,  speeifioally  the  extended  Kalman  filter  (EKF), 
optimizes  aiding  aetion  without  the  need  of  a  bateh  estimator.  This  strategy  reduees  the 
hardware  requirements  and  inereases  the  robustness  and  applieability  of  the  design 

This  ehapter  develops  the  error  propagation  model  and  bearings-only 
measurement  model  speeifie  to  the  SLAAMR  algorithm.  The  observability  and  measure 
of  aiding  aetion  provided  by  bearings-only  measurements  are  explored  through  the 
examination  of  the  observability  grammian  matrix  and  geometrie  dilution  of  preeision. 
This  observability  study  identifies  and  proposes  solutions  to  the  weaknesses  of  a  SLAM- 
based  system.  The  proposition  that  INS  drift  ean  be  slowed  by  traeking  self- 
loeated/surveyed  landmarks  is  diseussed  and  a  praetieal  strategy  is  developed.  The 
eritieal  problem  of  using  monoeular  imagery  to  resolve  the  3 -dimentions  modeled  in  an 
EKF  is  diseussed,  and  praetieal  solutions  are  proposed.  Finally,  the  prineiples  of  the 
SFAAMR  algorithm  design  are  developed,  and  summarized  along  with  the  algorithmie 
arehiteeture.  The  SFAAMR  algorithm  differs  from  traditional  SEAM  in  that  it  departs 
from  theoretieal  eonjeeture  and  is  designed  to  solve  praetieal  problems  posed  by  realistie 
implementation  limitations. 

Figure  3.1  illustrates  the  underlying  prineiple  that  allows  SFAAMR  to  work.  An 
aireraft  using  an  onboard  INS  estimates  its  position,  veloeity,  and  attitude  (eolleetively 
the  navigation  state  or  nav-state)  as  it  flies,  but  this  estimate  drifts  from  truth.  Along  the 
way,  a  eamera  is  used  to  traek  stationary  landmarks  on  the  ground.  The  position  of  these 
landmarks  is  either  known  or  estimated.  The  navigation  state  and  landmark  positions  are 
used  to  estimate  where  the  landmark  should  appear  in  sueeessive  images.  The 
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differences  between  actual  landmark  image  projections  and  the  estimates  are  used  to 
resolve  a  3 -dimensional  position  error,  which  correlates  to  error  in  the  aircraft  navigation 
state. 


True  aircraft  position  at  ti+i 


This  chapter  discusses  the  theory  behind  the  SLAAMR  algorithm  and  develops 
the  case  for  its  underpinning  assumptions  and  tenet  of  design.  A  rigorous  study  of  the 
underlying  measurement  equations  and  navigation  state  observability  follows. 
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3.1  Basic  EKF  System  Model 

The  underlying  meehanism  for  SLAAMR  is  a  low-eost  inertial  navigation  system 
(INS)  aided  by  bearings-only  image  (and  other  passive)  measurements,  via  a  feedbaek 
error  state  modeled  by  an  extended  Kalman  filter  (EKF).  Chapter  2  fully  developed  the 
EKF  model  and  proeess,  and  this  ehapter  will  develop  the  speeifie  propagation  equations 
and  measurement  models. 

3.1.1  Error  State  Propagation  Model.  The  goal  of  SEAAMR  is  to  aid  low-eost 
INS  implementing  a  low-quality  miero-maohined  eleotromeohanieal  systems  (MEMS) 
inertial  measurement  unit  (IMU).  This  allows  the  assumption  of  a  flat  and  non-rotating 
Earth  approximation  for  the  error  propagation  model.  As  derived  in  Chapter  2,  the  base 
error  propagation  model  for  SEAAMR  is: 

^x(t)  =  F  •  ^x(t) -r  G  •  w  (3.1) 

where 

O3  "^ne  O3  O3  O3 

O3  O3  qfx  Cl  O3 

O3  O3  O3  O3  -Cl 

1  (3.2) 
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and 


O3  O3  O3  03“ 

c:  03  03  03 

O3  -c:  O3  O3  (3.3) 

03  03  I3  03 

^3  *^3  O3  h_is^j2 


where,  transforms  veloeity  errors  from  units  of  m/s  to  geodetie  equivalents,  f*  x  is  the 
skew  symmetrie  form  of  the  speeifie  foree  veetor  resolved  in  the  b-frame,  is  the  b- 
frame  to  n-frame  DCM,  and  are  the  aeeelerometer  and  rate  gyro  bias  time  eonstants. 
The  15-state  error  veetor  ( 5x(t) )  is  also  defined  in  Chapter  2: 


5x  =  [<Jp  e;  a‘  (3.4) 


This  basie  navigation  error  state  veetor  will  be  augmented  to  inelude  landmark  position 
estimates  in  Seetion  3.2.4.  The  dynamies  driving  noise  assoeiated  with  the  inertial 
sensors  and  their  bias’  ( w )  is  defined; 


O3  O3  O3 

O3  O3  O3 

O3  O3  Q,  O3 

O3  O3  O3  Qj 


(3.5) 
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where  and  are  the  3x3  noise  strength  (intensity)  matriees  for  aoeelerometer  and 


rate  gyro  (triad)  measurement  noise,  and  and  are  the  3x3  proeess  driving  noise 

strength  (intensity)  matriees  for  the  aeeelerometer  and  rate  gyro  (triad)  bias  drift  rates 
(modeled  as  first  order  Gauss-Markov  proeesses). 

3.1.2  Linearized  Measurement  Model.  As  deseribed  in  Chapter  2,  the 
SLAAMR  measurement  model  takes  the  form; 


z(t.,i)  =  h[x(t,vX^M],  +H[t,,i;x(t,,r)].  5x(t.,j)  +  v(t.,i) 


(3.6) 


where  h[x(t,,r  is  the  non-linear  measurement  equation,  and  H[t;^j;x(t.^j  )]  is  the 
matrix  partial  derivative  of  h.  z(t.^j)  is  the  feature  position  measurement  expressed  in 
the  image  frame,  and  v  is  the  measurement  noise  driven  by  system  opties,  ete.  Again, 
5x(t.)  will  be  augmented  to  inelude  landmark  position  estimates  (diseussed  in  Seetion 

3.2.4),  but  the  form  of  Equation  (3.6)  remains  the  same.  The  following  seetions  develop 
this  measurement  model  for  bearings-only  image  measurements. 

3.1.3  Initial  Landmark  Position  Estimation.  Before  an  EKE  ean  begin,  it 
requires  an  initial  estimate  of  the  state  variables  and  assoeiated  eovarianees.  In  the 
feature  traeking  ease,  the  state  variables  are  the  navigation  and  traeked  objeet  position 
error  states.  Additionally,  the  EKE  requires  an  estimated  nominal  trajeetory  in  order  to 
perform  the  linearization.  This  trajeetory  ineludes  navigation  and  traeked  landmark 
positions.  Initial  estimates  of  the  navigation  whole  and  error  states  are  obtained  from 
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traditional,  well-established  INS  alignment/transfer  alignment  teehniques,  and  will  not  be 
diseussed  here  [23].  Initial  objeet  position  estimates  must  be  derived  from  one  of  two 
eategories  of  teehniques:  third  party  loeation  (surveyed),  and  auto-loeation  (self- 
surveyed).  The  initial  position  estimate  provides  a  nominal  trajeetory  about  whieh  the 
EKF  error  model  is  linearized.  The  initial  error  in  this  position  is  assumed  to  be  a  zero- 
mean  and  Gaussian  (with  an  assoeiated  eovarianee  matrix).  Subsequent  iterations  of  the 
EKF  refine  the  initial  position  and  error  eovarianee  estimates. 

3. 1.3.1  Third  Party  Survey.  When  a  third  party  souree  is  used  to  loeate  a 
landmark,  any  errors  in  the  estimated  landmark  position  are  independent  of  the  EKF 
navigation  state  errors.  Thus,  the  uneertainty  of  the  position  estimate  is  strietly  a  funetion 
of  the  third  party  system.  Determining  an  objeet’s  loeation  through  satellite  imagery  or  a 
GPS  site  survey  are  examples  of  this  method.  Beeause  the  aireraft  and  its  sensors  play  no 
part  in  this  proeess,  the  errors  assoeiated  with  third  party  survey  are  eompletely 
independent  of  the  navigation  state  and  measurement  equation.  In  the  ease  of  differential 
GPS  position  estimates,  the  uneertainty  eovarianee  ean  be  sub-meter  and  elliptieal.  These 
types  of  landmarks  will  heneeforth  be  known  as  surveyed  landmarks. 

3. 1.3. 2  Self-Survey  or  Auto-location.  Self-survey  or  auto-loeation  involves 
determining  the  position  of  a  landmark  of  opportunity  (EOO)  through  measurements 
taken  from  onboard  sensors,  in  this  ease  a  eamera.  The  eamera  measures  the  relative 
position  of  the  landmark  to  the  aireraft.  This  relative  position  is  then  added  to  the  aireraft 
position.  This  teehnique  has  been  demonstrated  through  the  use  of  binoeular  vision, 
multiple  image  bateh  estimation,  or  aetive  sensors  (sueh  as  an  EO/IR  targeting  pod)  [26]. 
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Single  image  monoeular  teehniques  are  the  primary  method  explored  herein,  beeause 
monoeular  imagery  is  a  more  practical  and  low-cost  option  for  aircraft.  Whichever 
technique  is  used,  the  measurement  equation  for  auto-location  estimates  take  the  form; 

Prg.  =4g.+PiNs  (3-V) 

where  the  initial  landmark  position  estimate  (p^g^)  is  the  sum  of  INS  calculated  aircraft 
position  (Pins)  the  relative  position  measurement/estimate  The  relative 

position  measurement/estimate  is  a  function  of  the  whole  INS-provided  navigation  state 
(x^)  and  the  sensor-produced  measurement/estimate  (z). 

A.,.=h,(x,z)L^_  (3.8) 

where  is  a  non-linear  function  used  to  determine  the  landmark’s  relative  position. 
Specific  examples  of  are  discussed  in  Section  3.5.  The  associated  uncertainty  of  the 

object  position  estimate  is  derived  through  perturbation  analysis  of  this  equation.  The 
error  perturbations  are  related  as: 

Ax 
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These  errors  are  assumed  to  be  zero  mean  and  Gaussian.  Therefore,  the  error  eovarianee 
of  the  objeet  position  estimate  is  equal  to  the  expeeted  value  of  the  outer  produet  of  the 
errors. 


P„,=E[Sp,JpJ] 

(SA,^,+Sp)(SA,^,+Spf 

■■  E  [S^g^SAj  ]  +  E  [SA^Jp^  ]  +  E  [5pdAj  ]  +  E  ] 


(3.10) 


The  aireraft  position  errors  are  independent  of  the  relative  position  estimate,  but 
in  praetiee,  the  measurement  equation  is  evaluated  about  the  nominal,  errant  navigation 
state.  The  assumption  is  made  that  nominal  is  close  to  truth.  Thus,  the  aireraft  position 
errors  are  approximated  to  be  independent  of  the  relative  position  estimate.  The 
eovarianee  expression  simplifies  to: 


Kt  =  E  ]  +  E  [^p^p^  ] 


+E 


^X 

^X 

dz 


SxSx 


SzSx 


5z 

^x 

^X 


^x 


5z 
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e[^p^p^] 


+  E 

^X  dz 

+  E 

dz  dz 

(3.11) 
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where  Ppp ,  and  P^^  are  the  aireraft  position,  navigation  state,  and  measurement  error 
eovarianees,  respeetively.  P^^  and  P^^  are  the  eross  eorrelation  matriees  between  the 
navigation  state  and  the  measurements.  and  are  the  partial  derivatives  of  (x,z) 
with  respeet  to  x  and  z,  evaluated  at  x^and  z.  The  eamera  optieal  uneertainties  are 
independent  of  the  navigation  state  errors,  allowing  the  simplifieation; 

P.,.=H.P..H/+H,P,,HJ+Ppp  (3.12) 

In  the  case  in  which  a  perfect  relative  measurement  can  be  taken  ( =0),  the 

uncertainty  in  the  object’s  initial  position  estimate  can  be  no  better  that  the  uncertainty  in 
the  aircraft’s  own  position  estimate.  Precise  optics  and  continued  bearings  measurements 
could  theoretically  refine  to  a  near  perfect  estimate,  but  since  this  is  a  relative 

estimate,  the  uncertainty  contribution  of  the  aircraft’s  own  position  will  always  remain. 
Thus,  the  lower  limit  to  LOO  position  estimate  certainty  is  driven  by  Ppp .  A  tracked 

LOO  position  estimate  uncertainty  can  approach,  but  never  exceed  that  of  the  host 
aircraft.  When  considering  surveyed  landmarks,  the  reverse  is  true;  the  aircraft’s  position 
uncertainty  can  approach,  but  never  exceed  that  of  the  best  surveyed  landmark. 
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3,2  Feature  Tracking  Measurement  Model 

Tracking  features  in  suceessive  images  is  the  underpinning  proeess  that  will  allow 
the  SLAAMR  algorithm  to  operate.  It  involves  multiple  steps  from  generating  features, 
to  matehing  them  eorreetly,  and  eventually  mapping  their  measurements  into  the 
navigation  state.  For  a  landmark  projeeted  onto  an  image  to  be  tracked,  two  things  must 
oeeur:  the  assoeiated  feature  must  preeisely,  aeeurately  eorrespond  to  the  same  geodetie 
position  in  each  frame,  and  it  must  be  correctly  and  consistently  identified. 

3.2.1  Feature  Generation.  In  his  work,  Veth  implemented  a  seale-invariant 
feature  transform  (SIFT®)  ereated  by  Lowe  [26][II].  The  SIFT®  generates  ‘key  points’ 
for  distinet  features  in  an  image.  Eaeh  key  point  eontains  a  eamera  frame  position  (in 
pixel  eoordinates),  seale,  orientation  and  a  distinet  deseriptor  allowing  matehing  to  oeeur. 
Veth’s  work  demonstrated  that  SIFT®  is  a  praetieal  and  funetional  feature  generator  and 
was  used  in  the  research  herein.  Figure  3.2  shows  an  image  from  flight  test  with  overlaid 
features  generated  by  SIFT  .  Feature  seale,  orientation  and  deseriptor  data  are  not 
shown. 
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Figure  3,2:  Flight  test  image  with  overlaid  SIFT©  generated  features.  The  features 
are  identified  hy  a  dot. 


3.2.2  Feature  Matching.  The  descriptor  generated  by  SIFT  is  a  distinct  128- 
element  vector.  Figure  3.3  plots  the  128-descriptor  values  versus  the  element’s  location 
(for  one  specific  feature),  compares  it  to  a  matched  feature  in  another  image  and  a  feature 
that  does  not  match. 


91 


0.35 


Reference  Feature 

—  —  Matched  Feature  (corr  coeff  =  0.94) 


Unmatched  Feature  (corr  coeff  =  0.44) 


128  elements 

Figure  3.3:  Representative  descriptor  values  versus  their  element  location. 


The  nature  of  the  deseriptor  allows  aeeurate  matehing  using  an  auto-correlation 
routine.  A  correlation  coefficient  (/?  )  is  derived  for  a  frame-to-frame  feature  comparison 
via  the  dot  product  of  the  two  feature  descriptor  vectors,  normalized  by  the  dot  product  of 
the  first  feature  descriptor  with  itself  The  correlation  routine  implemented  is 


P  = 


(3.13) 


where  d^  and  d^  are  the  descriptor  vectors  of  the  two  features  being  compared.  The 
coefficient  p  varies  from  1  (perfect  match)  to  potentially  -1.  Practically,  the  range  is 
0. 5-1.0  because  of  the  generally  positive  descriptor  element  values.  A  match  is  declared 
if  p  exceeds  a  threshold  derived  for  a  good  match. 

The  use  of  SIFT  allows  the  SLAAMR  algorithm  to  operate  in  nearly  any 
environment,  and  travel  long  distances  without  the  need  to  revisit  old  landmarks. 
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Previous  work  on  SLAM  algorithms  used  a  set  of  pre-plaeed  targets  in  a  confined  area 
[5].  The  targets  were  found  and  matched  using  a  template  matching  algorithm.  This 
approach  worked  well,  but  limits  environment  in  which  it  can  operate  and  relies  on  a 
priori  information  about  the  targets  (appearance)  despite  not  knowing  their  location. 

For  any  given  image,  SIFT®  produces  hundreds  to  thousands  of  features. 
Applying  the  correlation  equation  to  every  feature  (in  order  to  find  a  match)  is 
computationally  impractical  and  could  lead  to  potential  false  matches.  The  EKF 
naturally  predicts  the  geodetic  position  of  all  tracked  landmarks  and  assigns  an 
uncertainty  volume  to  that  prediction.  Projecting  this  estimated  position  and  uncertainty 
in  subsequent  images  allows  the  SLAAMR  algorithm  to  spatially  constrain  which 
features  will  be  considered  for  a  match.  The  3 -dimensional  landmark  position  and 
uncertainty  project  into  the  image  as  a  2-dimensional  pixel  location  and  corresponding 
ellipsoid.  Figure  3.4  illustrates  this  principle. 

The  cross  and  ellipse  represent  the  estimated  feature  projection  and  3 -sigma 
uncertainty  bound  (3  standard  deviations  of  '-99%  confidence).  Only  features  that  fall 
within  the  ellipse  are  considered  for  a  match.  If  none  exceed  the  threshold  for  a  good 
match,  the  track  is  considered  broken. 
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Figure  3,4:  Constrained  feature  search  volume. 


The  projection  of  landmark  position  and  uncertainty  into  the  image  is 
accomplished  via  the  measurement  vector  (h[x(^,  ),^J)  and  observation  matrix 


( )] ),  which  are  developed  in  the  next  section.  The  general  form  of  the  estimated 
landmark  projection  is 


z(0  =  h[X|Ns(0] 


(3.14) 
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where  is  the  estimated  projection  expressed  in  pixel  coordinates,  and  is 

the  INS  derived  navigation  state.  The  uncertainty  in  the  estimated  projection  is  mapped 
into  the  image  by: 

Pzz  =H[x„s(g]-P,,  •H[x„3(gf  +R  (3.15) 


where  is  a  2x2  covariance  matrix  describing  the  2-dimentional  uncertainty  ellipse, 
is  error  state  uncertainty  covariance  matrix  and  R  is  the  measurement  noise 
covariance  matrix. 

3.2.3  Image-Only  Measurement  Equation.  As  described  in  Chapter  2,  the 
position  vector  of  a  tracked  landmark,  resolved  in  the  c-frame  (s^^),  is  a  function  of 
aircraft  and  landmark  geodetic  position  (p  and  ),  camera  mounting  position  and 

alignment  (relative  to  the  IMU,  p*^^  and  C*),  and  aircraft  navigation  state  derived 
transformations  (T^„  and  C^),  where  =C^C* : 


T,„(p.g.-p)-c:p: 


b 

cam 


(3.16) 


where  transforms  the  relative  difference  between  aircraft  and  the  tracked  landmark 

position  from  geodetic  units  into  meters.  The  relative  position  vector  between  the  tracked 
object  and  the  aircraft  can  be  expressed  as: 

a=t,.(p,^,-p)-c;p1  (3.17) 
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The  c-frame  object  position  vector  is  transformed  into  the  homogeneous  image 
measurement  expressed  in  camera  pixel  location  coordinates: 


z  =_T^“s" 


_  ^  ^pix^c 


T,„(P.g.-p)-C:P 


b 
cam 


(3.18) 


If  the  magnitude  of  is  small  (as  it  was  in  this  study),  it  can  be  ignored  in  order  to 
simplify  calculations.  This  step  is  not  required,  nor  does  the  inclusion  of  a  non-trivial 
PLm  change  the  problem  or  analysis.  The  scalar  si  is  the  z-component  of  the  object 
position  vector.  It  can  be  expressed  as: 

5:=[0  0  I]C:a  (3.19) 

The  projection  of  the  object’s  image  onto  the  image  plane  is  defined  as: 

S™=[x/,1V/]’  (3.20) 

and  is  related  to  the  c-frame  object  position  vector  ( )  as  follows 


^proj 


'P 


(3.21) 
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Substituting  into  Equation  (3.18)  yields 


(3.22) 

Equation  (3.22)  is  over-determined,  allowing  a  simplification.  Optical 
measurements  have  the  ^"strength”  of  two  equations  only;  image  projections  onto  a  focal 
plane  are  two-dimensional,  is  a  homogeneous  vector,  and  there  is  no  need  to 

calculate  the  third  component  (which  is  a  constant  1).  also  has  a  known  third 

component,  the  focal  length.  These  properties  simplify  the  measurement  equation 
development. 

The  c-frame  object  position  vector  (s^^)  and  image  projection  vector  (s^™^  )  form 
similar  triangles  and  are  scaled  by  the  ratio  of  their  lengths. 


1 


T/ 

/ 


(3.23) 


where  the  length  of  the  and  are  defined  by  the  3-D  norm 


\\s"\\  =  ^x/+y;+f- 

Because  is  unitary 

||s1|=IHI=V^/+V+V 


(3.24) 


(3.25) 
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Combining  equations  yields: 


7/ 

/ 


+y/  +f^ 

w 


■C!A 


(3.26) 


The  focal  length  is  the  third  component  of  the  vector  on  the  left  hand  side  of  Equation 
(3.26),  and  can  be  expressed  as 


/  = 


^x/+y/+f‘ 

w 


[0  0  i]c;a 


(3.27) 


Equation  (3.27)  is  used  to  eliminate  the  scalar  on  the  right  hand  side  of  Equation  (3.22). 
Isolating  the  unknown  scalar  (which  also  features  the  range  to  the  landmark)  yields: 


yjx/  +y/  +f^  _  / 

PI  “[0  0  1]C:a 


(3.28) 


Substituting  into  the  original  relationship  yields  the  two  measurement  equations: 


Xf^ 

f 

“1  0  o' 

[0  0  i]c:a 

0  1  0 

(3.29) 
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By  dropping  the  homogeneous  third  component,  the  two-dimensional  measurement 
equation  vector  ( )  becomes 


1  0  0 
0  1  0 

1  0  0 
0  1  0 


/ 


/ 


7/ 

/ 


(3.30) 


3.2.4  Augmented  State  Vector  for  Landmark  Position  Estimation.  Augmenting 
the  navigation  state  vector  with  the  landmark’s  position  vector  allows  the  measurement 
model  to  fit  the  Kalman  filter  mold  described  in  Chapter  2.  It  is  also  important  to  do  this 
in  order  to  model  the  errors  in  the  knowledge  of  the  landmark’s  position,  for  although  it 
does  not  move,  its  true  location  cannot  be  known  for  certain.  Thus,  the  augmented  state 
vector  is 

’  P 

v: 

^  nb 

Note  that  the  accelerometer  and  gyro  biases  are  not  included  in  the  navigation  state  vector 
for  this  discussion.  These  bias  states  are  normally  included  in  the  state  vector,  but  play 
no  part  in  optical  measurements.  For  the  sake  of  readability  and  without  loss  of 
generality,  they  are  thus  excluded  from  this  discussion. 


=  L  I  h  (p  e  y/  \,]^(3.31) 
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The  associated  augmented  error  state  vector  is  then: 


^\dL  SI  Sh  Sv^  Svi,  Svj,  £•  i  SI^^  (3.32) 

3.2.5  Linearized  Image-Only  Measurement  Equation.  In  order  to  implement  an 
EKF,  the  measurement  equation  must  be  linearized.  Recall  the  form  of  the  non-linear 
measurement  model  [15]. 

z/0  =  h[x(0]  +  v(0  (3.33) 


'^nb 


tg 


Since  A,C^  are  derived  from  the  navigation  state  x(t) ,  the  deterministic  portion  of  z^(t) 
( h[x(t)])  can  be  re-written: 


h[x(0]  = 


1 

o 

o 

•jpix 

Xj 

0  1  OJ 

c 

f 

_ _ _ _ J 

(3.34) 


,  and  /  are  constant,  thus  a  simpler  measurement  equation  may  be  considered 


h'[A,C:]  = 


.yf. 


(3.35) 
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where 


and 


h[x(0]  =  h(h'[A,C^]) 


"1  0  0^ 

-rpix 

*  c 

'h'[A,C:f 

0  1  OJ 

f 

/ 

(3.36) 


z/0  =  h(h'[A,C:])  +  v(0 


(3.37) 


Because  T/“  and  /  are  constant,  the  partial  derivative  of  h[x(t)]  with  respect  to  x(t)  is 


the  partial  derivative  of  h'[A,C^]  with  respect  to  x(t)  (or  equivalently,  A  and  C^) 
multiplied  by  a  constant  matrix. 

To  develop  the  linearization  of  the  measurement  equation,  unit  vectors  must  first 
be  introduced: 


T 

“0^ 

“O' 

0 

5  “ 

1 

5  “ 

0 

OJ 

OJ 

1 

(3.38) 


The  measurement  equation  can  now  be  written  more  compactly: 


h'[A,C:]  = 


yf. 


(3.39) 
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To  linearize  the  measurement  Equation  (3.39),  the  partials  with  respeet  to  the  state 
variables  must  be  taken.  The  partials  with  respeet  to  A  equal  the  partials  with  respeet  to 
the  relative  eamera  and  landmark  positions.  Also,  for  the  relatively  small  changes  in 
geodetic  position  over  time  (that  a  conventional  aircraft  undergoes  in  flight),  the 
transform  between  geodetic  and  n-frame  units  of  measurement  (T^„)  is  treated  as  a 

constant  matrix.  This  simplifies  the  solution  only  somewhat.  The  following  is  the 
mathematical  derivation  for  the  perturbation  model  used  to  linearize  the  measurement 
model  about  the  INS  calculated  trajectory. 

Now,  the  measurement  equation  elements  are 


h[ ( A,C; )  =/  ' 


(e/c;A) 


A'C!e, 


(e3"C:A) 


A'c:e3 


(3.40) 


(3.41) 


This  form  of  the  measurement  equation  facilitates  the  calculation  of  the  partial 
derivatives.  Each  element  ( and  )  of  the  measurement  equation  takes  the  form: 


T  = 


(3.42) 


where  a  and  b  are  constant  vectors. 
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The  perturbation  in  j  with  respect  to  Sx  is  thus: 


Sy  =  —Sx  =  — (ba"^  -ab'^)^x 
5x  (x^b)^  ^  ^ 


(3.43) 


In  Equations  (3.40)  and  (3.41),  let  x  =  A,  a  =  C”ej2,  and  b  =  C"e3.  This  substitution 
yields: 


SI,',  =  xUba"  - ab") <Jx=;/' « 

(x’b)’-  '  '  ■'  (A^c;e3) 


(3.44) 


The  component  due  to  perturbations  in  attitude  requires  further  development. 
Consider  the  scalar  function 

y  =  h'(x),  (3.45) 

where  the  variable  of  interest  can  be  written  as  the  nonlinear  relationship 

x  =  a^C"b  (3.46) 

Here,  a  =  A  and  b  =  Gj  2  3  •  Both  are  3  dimensional  column  vectors.  The  perturbation  in 
y  caused  by  a  perturbation  in  the  DCM  is: 

Sy=^a^sc:b  (3.47) 

ax 
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Small  changes  in  the  DCM  are  driven  by  the  small  changes  in  the  camera  attitude  angles’ 


error  veetore”^ .  The  following  holds  true  about  the  perturbed  DCM  [23]; 


C” 

'^cperturbed 


fl-fe"  x))c"=C"-fe"  x)C" 

\  \  nc  Jj  c  c  \  nc  }  c  c  c 


(3.48) 


Thus  it  ean  be  deelared  that: 


=-fe"  x'lC” 

c  \  nc  J  c 


(3.49) 


Hence,  the  perturbation  my  eaused  by  is: 


dy= 


5x 


x'lC" 

\  nc  }  c 


(3.50) 


Since  the  camera  is  rigidly  connected  to  the  b-frame,  and  are  errors  about  the  n-frame 
axis,  is  equivalent  to  the  aircraft  attitude  angles’  error  veetor  .  Substituting  the 
variables  yields: 

C:e,)+A^C;e, 

(A^C:e,( 


Sy=f- 


A'C" 


(Aqe;,x)c>,) 


(3.51) 
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Using  the  identity  (e^  = 


Gg ,  e"j  is  pulled  out  to  the  right  hand  side  ; 


A"c:  e3A"  c:e,  x-e,A^  C:e3  X 

^y=/ — ^ — \  r  — —< 

A^c:e3 


(3.52) 


where  (^C"e,Jx  is  the  skew  symmetric  form  of  the  transformed  vector  C"e. .  Combining 

all  the  perturbations  for  the  optical  measurement  Equations  (3.40)  and  (3.41)  yields  the 
perturbation  model  with  respect  to  the  navigation  state  errors: 


,A^C:e,e7c:-A^C;e,e;c.^^^A^C;(e,A^(c;e,)x-e,A^(C:e,)x) 


(A’C;e,) 


(A^qe,) 


'nb 


(3.53) 


.  /  [A'q(e,e7-e,e3^)qa+A^C:(e3A^(qe,)x-e,A^(c;e,)x)e;] 

(ATc»e3)  V  V  /  V  /  /  j 


To  simplify,  the  unit  vectors  can  be  collected  as  such: 

6361"  -6163"= -©2  X 

6363^ -6363^  =eix 

The  skew  symmetric  form  of  the  unit  vectors  are  defined 
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Recall,  the  navigation  error  state  augmented  with  the  uncertainty  in  the  landmark 
position  is 

dY.  =  [dL  dl  dh  :  dv^  Sv^  dL,^,  (3.56) 


making 

^  {t,„  (p,,,  -  p)  -  qpl ) = -  p))  -  ^  (c^pI  ) = (sp,^,  -  ^p) 


K  dl  dh^) 


(3.57) 


Thus  the  simplified  observation  matrix  is  written: 


HWO] 


0,,  ;  A^q(e,A'(cre,)x-eiA^(c;e,)x)  ;  aV^ct." 

o„  ;  A'c;(&A'(c;e,)x-ey(qe3)x)  ;  ^■'q^CT,_ 

(3.58) 


The  complete  observation  matrix  is  obtained  by  applying  Equation  (3.36): 


H[x,(0] 
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(3.59) 
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The  transform  merely  ehanges  the  units  from  geodetie  (radians  of  longitude  and 

latitude)  to  meters,  and  is  eonsidered  essentially  eonstant.  Without  loss  of  generality,  it 
can  be  pulled  out  of  Equation  (3.59),  and  applied  to  the  error  states  before  the  Kalman 
filter  update  cycle.  Also,  a  reference  frame  can  be  declared  such  that  is  identity.  The 

same  can  be  said  for  the  constants  in  front.  For  the  sake  of  readability,  and  the 

constants  in  front  will  be  omitted  from  the  observation  matrix  from  this  point  on. 

3.2.6  Tracking  Many  Unknown  Landmarks.  Tracking  of  multiple  landmarks 
with  position  that  is  unknown  simply  requires  augmenting  the  error  state  (and  associated 
state  vector)  with  additional  landmark  position  states: 


p 

v" 

^  eb 

\T/  n 

^  nb 

,  = 

<b 

_^P.g.N_ 

(3.60) 


where  p,  v  and  'F  are  the  states  associated  with  the  INS,  and  is  the  n'^  landmark 
position  vector. 
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The  error  propagation  equations  are  augmented  to  model  the  stationary  objeets  as 


follows: 


^MxM 

^Mx3 

^Mx3 

^Mx6 

II 

•X 

^3xM 

Sx  + 

^3x6 

_^3xM 

^3x3 

^3x3  _ 

_  0,.,  _ 

S\N 


(3.61) 


where  F  and  G  are  the  INS  (MxM)  systems  dynamies  matrix  and  the  {Mx6)  noise 
transformation  matrix  from  Chapter  2.  One  column  and  row  of  zeros  is  added  for  every 
new  object  position  state  variable  added. 

The  measurement  Equations  (3.40)  and  (3.41)  are  augmented  as  well  to  reflect  the 
added  measurements  and  state  variables.  The  observation  matrix  (H[x,(0])  is 
augmented  with  two  additional  rows  for  each  additional  tracked  landmark.  The  pair  of 
rows,  corresponding  to  the  of  N  tracked  landmarks,  are  defined: 
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(3.62) 
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3.2. 7  Tracking  Known  Landmarks.  If  the  landmark  position  is  known  perfectly, 
the  state  variables  eoneerning  the  objeet  loeation  ean  be  dropped,  and  substituted  for 
eonstants  throughout  the  model.  True  landmark  position  knowledge  is  reasonably 
attainable  through  an  a  priori  survey,  but  will  not  be  diseussed.  In  the  ease  in  whieh  a 
single  object  is  tracked,  the  object  line-of-sight  (LOS)  vector  can  be  substituted  for  the 
position  vector. 


p<- A  = 


(3.63) 


Thus  the  new  navigation  state  error  vector  is  now: 


(3.64) 


Because  the  LOS  vector  is  now  the  difference  between  the  camera  position  and  a  constant 
landmark  position,  the  position  and  velocity  derivatives,  and  therefore  the  error 
propagation  equations,  do  not  change  from  before: 


~SA^ 

~dA~ 
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II 
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e" 

J 

(3.65) 


The  measurement  equation  also  remains  as  described  before. 
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The  case  where  many  known  landmarks  are  tracked  is  a  simple  extension.  Since 
all  the  landmarks  are  known  and  stationary,  the  position  of  one  can  be  related  to  another 
by  differencing  their  positions. 

i  =  2,...,N  (3.66) 

Because  the  landmarks  are  stationary,  the  differences  in  their  positions  are  constant. 
Therefore,  no  matter  how  many  known  landmarks  are  tracked,  the  state  dimension 
remains  the  same.  The  LOS  vectors  A.  used  in  the  additional  measurement  equations  are 
therefore  expressed; 


A,=P -P 


tgt  i  ’ 


(3.67) 


The  augmented  measurement  equation  (h"[x(t)])  simply  becomes  a  vector  of  N 
functions  taking  the  form  of  Equation  (3.36). 


h"[x(0] 


h,[x(0] 

h^[x(0] 


(3.68) 
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The  Observation  matrix  then  eontains  N  pairs  of  rows,  where  the  pair  of  rows  are: 


A.^q(e,A.^(C;e,)x-e,AhC;e,)x) 

A^q(e3A"(qe4x^A"(qe,)x) 


(3.69) 


3.3  Image-Only  Measurement  Observability 

The  strength  of  aiding  action  provided  to  the  INS  from  optical  measurements 
depends  on  the  degree  of  observability  of  the  INS  error  states  as  provided  by  the 
measurement  equation.  The  observation  matrix  H(Xp(t))  is  time-dependent.  Hence,  the 

degree  of  observability  provided  by  bearings-only  measurements  requires  the  calculation 
of  the  observability  grammian 


=  (3.70) 

0 

Note  that  the  observability  grammian  is  also  trajectory-dependent.  A  study  of  the 
observability  of  bearings-only  measurements  for  a  SLAM  based  system  has  been 
accomplished  [30],  but  this  section  fully  develops  the  problem  for  an  airborne  application 
in  an  attempt  to  design  an  aiding  strategy  more  effectively.  It  is  important  to  note  that 
this  study  applies  to  the  update  cycle  of  the  EKF  and  speaks  nothing  to  the  error  growth 
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that  occurs  between  measurements.  Error  growth  that  oecurs  in  the  EKE  propagation 
eycle  is  a  function  of  the  IMEi  being  used.  This  error  is  constrained  most  effeetively 
when  the  measurement  equation  has  good  observability.  This  study  also  examines  the 
eontinuous-measurement  case,  but  the  findings  can  be  extended  into  the  discrete,  still- 
eamera  realm  in  good  faith. 

3.3.1  Surveyed  Landmark  Tracking  Observability.  It  is  useful  to  begin  with  the 
simpler  condition  of  tracking  landmarks  with  perfectly  known  positions  (nav-state  not 
augmented  for  landmark  position  estimation).  Consider  wings-level,  constant- altitude 
flight.  A  downward  looking  camera  is  employed  from  the  aireraft  flying  at  a  constant 
velocity  (v)  and  constant  altitude  {h)  over  flat  terrain  for  time  {t).  A  landmark  direetly 
along  the  flight  path  is  tracked.  The  INS  aiding  action  may  be  thoroughly  examined  in 
this  simple  scenario. 

The  camera,  body,  and  nav-  frames  are  all  nominally  aligned.  Thus,  =  I .  In 

the  ease  where  one  known  landmark  is  tracked,  the  9x9  observability  grammian  for  level, 
un-aeeelerated  flight  is 
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(3.71) 
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where  A(^J  =  [a^  A^  A^]^  ,  ^^[t)  =  ^+v■t  ,  A/^;  =  0  ,  A^=A. 

Additional  variables  are  the  camera  focal  length  (/)  and  gravitational  constant  (g). 

Here,  the  components  of  the  specific  force  resolved  in  the  body  frame  ( /^ , 
and  )  are  defined;  f^=  /,,  =  0 ,  and  =-g ,  and  thus  not  readily  apparent. 

Additionally,  the  velocity  component  is  strictly  along  the  b-frame  x-axis  as  aircraft 
generally  fly  with  no  sideslip.  Drift  due  to  wind  is  ignored.  This  can  be  done  without 
loss  of  generality  as  an  arbitrary  body  frame  can  always  be  redefined  to  align  with  any 
constant  velocity.  To  increase  the  readability  of  the  above  matrix,  the  tracked  landmark 
was  chosen  to  be  along  the  flight  path  ( A^/tj  =  0).  Through  analysis  it  was  found  that 

this  restriction  did  not  affect  the  rank  of  the  observability  grammian  matrix.  Following 
matrices  are  less  complex  and  the  A^ftj  =  0  restriction  is  omitted. 

Landmarks  must  be  tracked  over  time.  Note  that,  as  time  goes  on,  the  elements  of 
the  grammian  grow  polynomially.  Additionally,  the  entire  matrix  is  scaled  by  t  (factored 
out,  and  multiplying  the  whole  matrix).  This  implies  the  great  benefit  of  long  observation 
periods. 

There  is  a  suggestion  of  scalability  as  well.  It  is  evident  by  the  l/ term  factored 
out,  and  multiplying  the  whole  matrix.  This  suggests  that  the  absolute  altitude  plays  as 
an  important  role  as  the  angular  geometry,  and  will  be  evaluated  with  simulation  in 
Chapter  4. 

To  calculate  the  rank  of  the  observability  grammian,  its  Row  Reduced  Echelon 
Form  (RREF)  is  obtained: 
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(3.72) 


where  the  position  of  the  tracked  landmark  is  not  confined  to  the  flight  path 
=  ).  Notice  that  in  the  RREF,  all  reference  to  time  drops  out.  Tracking  a 

landmark  for  long  period  does  not  affect  rank,  but  will  affect  matrix  condition  number. 

The  9X9  grammian  matrix  is  not  of  full  rank  9,  but  has  a  rank  of  6  for  all  time. 
Only  a  partial  state  estimate  can  be  obtained.  Tracking  a  single  known  landmark  will  not 
enable  one  to  aid  all  9  INS  states.  Additional  independent  measurements  of  the 
navigation  state  are  needed  for  full  INS  aiding  action  (full  rank). 

In  the  RREF  of  the  grammian,  the  forms  hiv,  A^  / v  and  A^  / v  appear  quite 

often.  The  ratio  of  velocity  to  lateral  displacement  and  ratio  of  velocity  to  altitude  (V 
over  h,  as  it  is  more  commonly  known)  plays  an  important  role  in  INS  aiding  using 


115 


bearings-only  measurements.  V  over  A  is  a  measure  of  image  apparent  angular  rate  and 
has  units  of  radians  per  see  (see'^).  This  parameter  ultimately  drives  the  measure  of 
geometrie  dilution  of  preeision  (GDOP)  and  observability  grammian  eondition  number. 
By  eontrolling  V  over  h,  one  eannot  affeet  rank  per  se,  but  ean  maximize  aiding  benefit 
and  distribution. 

In  the  ease  in  whieh  the  object  is  directly  over-flown  (A^  =0),  the  rank  remains 

6  as  well.  This  implies  that  the  lateral  position  of  the  landmark  does  not  affect  the  overall 
observability  of  the  system.  Geometry  does,  however,  affect  how  the  aiding  action  is 
distributed.  Better  insight  to  this  distribution  is  attained  by  examining  the  condition 
number  of  the  observability  grammian  and  GDOP. 

In  the  case  in  which  the  aircraft’s  trajectory  is  not  constrained  to  the  level,  un¬ 
accelerated  flight  regime  (where  the  specific  force  vector  is  aligned  with  the  z-axis),  the 
grammian  rank  increases  to  7,  and  the  RREF  is 
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(3.73) 


This  implies  that  accelerated  flight  does  benefit  INS  aiding  with  a  single  tracked 
landmark  of  known  position.  This  condition  requires  continuous  maneuvering  and  is, 
therefore,  impractical  for  typical  missions.  Accelerated  flight  is  more  favorable,  but  the 
level  un-accelerated  flight  condition  is  the  worst  case,  and  will  be  considered  from  hence 
forth. 

Tracking  one  landmark  does  not  provide  full  observability.  The  question  is  thus 
posed  whether  tracking  more  objects  will  improve  observability,  and  if  so,  how  many  is 
enough.  It  turns  out,  that  two  tracked  landmarks  are  enough  to  provide  full  grammian 
rank,  and  thus,  full  observability  and  INS  aiding  action.  The  following  development 
illustrates  this. 
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The  grammian  for  level,  un-aceelerated  flight  and  two  tracked  landmarks  can  be 
expressed  as  the  sum  of  two  grammians,  each  corresponding  to  a  single  tracked 
landmark. 


(3.74) 


where,  it  can  be  expressed 


—  Ai  +  Ppj  Pp2 


(3.75) 


The  grammians  W(t)|^  and  W(t)|^  correspond  to  the  two  tracked  landmarks,  and  take 

the  form  of  the  single  track  grammian  above,  but  evaluated  at  the  corresponding  relative 
landmark  position  vectors  A^  and  A^ .  The  observability  grammian  for  multiple  tracked 

landmarks  simply  becomes  the  sum  of  the  single  track  grammians,  evaluated  for  the  LOS 
vector  to  the  additional  landmarks: 


W,(t)  =  XW(t)|^  (3.76) 

i=l 

where  N  is  the  number  of  landmarks  being  tracked,  A^.  is  the  LOS  vector  to  the 
landmark,  W(t)is  the  single  landmark  grammian  from  Equation  (3.71),  and  W^(t)  is  the 
composite  observability  grammian  matrix  for  N  tracked  landmarks  of  known  location. 
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This  realization  greatly  simplifies  the  proeedure  for  evaluating  observability  grammian 
matrix  condition  number  when  tracking  many  landmarks. 

For  flight  in  the  vertical  plane,  and  without  the  level  flight  assumption,  the  RREF 
of  the  grammian  associated  with  the  tracked  landmark  is 
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(3.77) 


When  two  or  more  landmarks  are  tracked,  the  RREF  of  the  observability 
grammian  is  an  identity  matrix.  The  question  must  be  answered  as  to  whether  or  not  two 
tracked  landmarks  will  always  give  an  observability  grammian  with  full  rank.  The 
answer  is  affirmative,  but  there  are  two  exceptions.  The  grammian  has  full  rank  as  long 
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as  the  LOS  vectors  for  both  landmarks  are  not  co-linear,  and  the  aircraft  is  not  in  a  state 


of  free-fall  ( /^  =  /^  =  /^  =  0 ). 

The  LOS  vectors  for  both  landmarks  are  co-linear  if  the  following  is  true 

A,  =aAi  (3.78) 

where  a  is  a  scalar  multiplication  factor.  If  the  camera  and  the  two  landmarks,  all  lie  on  a 
straight  line,  the  observability  grammian’s  rank  drops  to  6.  This  is  intuitive,  as  both 
landmarks  provide  the  same  measurement  information  to  a  two-dimensional  camera;  the 
only  difference  between  the  landmarks  (in  the  camera  frame)  is  in  depth,  a  dimension  the 
camera  cannot  discern. 

The  camera  and  two  landmarks  are  on  the  same  straight  line  in  two  degenerate 
cases:  1)  the  aircraft  crosses  the  axis  defined  by  the  two  landmarks,  and,  2)  the  aircraft  is 
flying  along  this  same  axis.  In  the  first  case,  the  observability  drop  is  transitory.  As  the 
aircraft  crosses  this  axis,  the  grammian  rank  drops  momentarily,  but  then  returns.  The 
condition  of  the  observability  grammian  matrix  will  remain  poor  until  the  aircraft  is  far 
enough  away  to  ensure  good  geometry.  If  the  aircraft  travels  along  the  axis,  it  will  lose 
aiding  benefit  in  the  unobservable  dimensions;  position  and  velocity  along  the  axis,  and 
in  rotation  about  it.  This  is  illustrated  in  Figure  3.5. 
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Figure  3.5:  Unobservable  dimensions  for  two  landmark  tracks. 

An  INS  on  board  an  aircraft  traveling  along  the  axis  formed  by  the  two  tracked 
objects  will  receive  no  aiding  benefit  in  position,  velocity  and  rotation  along  and  about 
that  axis.  As  the  aireraft  approaehes  and  crosses  sueh  an  axis,  observability  drops  and 
aiding  suffers. 

The  second  exception  to  the  rule,  weightlessness  (/^_^2=0),  reduces  the 

observability  grammian  matrix  rank  to  8  (when  2  or  more  landmarks  are  being  tracked). 
For  typical  aircraft  applications,  weightlessness,  or  zero-g,  conditions  tend  to  be 
transitory,  and  should  not  cause  significant  aiding  degradation.  Free  fall  weaponry  using 
this  method  of  aiding  would  be  affected,  however.  Notably,  GPS  aiding  (of  an  INS) 
suffers  much  greater  than  landmark  tracking;  loosely  coupled  GPS  aiding  rank  drops  to  6 
at  zero-g  conditions. 

3. 3. 1.1  Non-Dimensional  Study  of  Aiding  Strength.  Full  observability  speaks 
to  the  existence  of  a  complete  aiding  solution,  but  says  very  little  of  the  quality  of  that 
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solution.  For  this,  GDOP  and  observability  grammian  matrix  condition  number  must  be 
studied.  The  GDOP  is  defined  as 


GDOP  =  ^trace{w(t)^'|  (3.79) 

where  W  (t)  is  the  observability  grammian  when  two  landmarks  are  being  tracked.  A 

high  condition  number  of  the  grammian  matrix  indicates  poor  geometry  and  consequently 
a  high  GDOP.  The  result  of  a  full  rank  grammian  with  high  condition  number  is  that  all 
states  are  being  aided,  but  the  aiding  action  is  poor  and  unevenly  distributed.  GDOP 
provides  a  scalar  measure  for  the  dilution  of  precision  attained  by  a  particular  geometry 
presented  to  the  measurement  device.  A  low  GDOP  value  indicates  little  dilution  of 
navigation  aiding  precision. 

It  is  important  to  non-dimensionalize  the  variables  and  problem  parameters  when 
studying  the  nature  of  the  observability  grammian  matrix.  This  ensures  scaling  issues 
don’t  obscure  the  insight  into  observability  gained  from  the  condition  number  or  GDOP 
calculation,  and,  hence,  INS  aiding  action.  The  following  development  explores  the 
effects  of  geometry  in  a  simple  scenario. 

The  scenario  considered  entails  an  aircraft  at  an  altitude  h  and  velocity  v,  traveling 
towards,  and  over-flying,  two  landmarks  arranged  in  a  line.  The  positions  of  the 
landmarks  are  expressed  as  a  multiple  of  the  aircraft’s  altitude,  as  shown  in  Figure  3.6. 
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Figure  3,6:  Flight  in  the  vertical  plane. 


The  aircraft  at  an  altitude  h  and  velocity  v  travels  towards  two  landmarks  arranged 
in  a  line;  their  positions  are  expressed  as  multiples  of  the  aircraft’s  altitude.  In  this 
scenario,  all  motion  is  confined  to  the  z-x  plane',  y  =  0,  though  3-dimensions  are  still 
maintained  in  the  state  vector. 

To  non-dimensionalize,  all  positional  variables  are  scaled  by  h.  Time  is  non- 
dimensionalized  as  follows: 


t'  ^ 


(3.80) 


and 


T'^ 


(3.81) 


where  T  is  the  duration  of  flight  and  g  is  the  Earth’s  gravitational  acceleration. 
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Aircraft  velocity  is  non-dimensionalized  by  setting 


(3.82) 


Aircraft  and  ground  object  positions  are  thus  expressed: 

Pac=[vt  0  1]"  p,=[a  0  0]"  P2=[p  0  of  (3.83) 


Finally,  the  specific  force  vector  is  also  normalized  by  dividing  through  by  g: 


f  =  [0  0  if 

Without  loss  of  generality,  we  constrain  P  to  be  greater  than  a . 


(3.84) 


—00  <  a  <  00 
a  <  P  <  00 


(3.85) 


The  parameters  a  and  P  determine  the  geometry  of  the  measurement  arrangement 

The  time  variable  is  constrained  to  those  instances  when  both  ground  objects  are 
in  view.  For  the  sake  of  generality,  assume  an  ideal  camera  with  a  180  degree  field-of- 
view  (FOV)  and  infinite  pixel  resolution.  The  camera’s  tracking  ability  is  not  limited  by 
the  geometry  of  the  objects. 
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Thus  the  four  non-dimensional  problem  parameters  are  a  ,  (5 ,  v'  and  T' .  The 


9x9  non-dimensionalized  dynamies  matrix  for  this  seenario  is 


O3 

I3 

O3  " 
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F  = 
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63  X 

,  e3x  = 
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0 

(3.86) 


and  the  relative  ground  object  position  vectors  needed  for  the  measurement  equation  are 

Ai=[v't-a  0  if  (3.87) 

and 

A2=[v't-p  0  if  (3.88) 

The  focal  length  (/)  must  also  be  divided  by  h  so  that  the  non-dimensional  focal 
length  is  small.  The  nominal  body-to-nav  frame  DCM  is  the  identity  matrix,  and  the 
altitude  (/?)  is  unity.  Hence,  the  observation  matrix  is 


H(x,(0)  =  / 


1 

0 

a-vt 

0 

0 

0 

0 

{v't-af  +1 

0 

0 

1 

0 

0 

0 

0 

-1 

0 

v't-a 

1 

0 

p-v7 

0 

0 

0 

0 

(v7-pf +1 

0 

0 

1 

0 

0 

0 

0 

-1 

0 

vT-p 

(3.89) 


125 


Choosing  the  geometry 


a  =  0, 

P  =  l, 

v'=l 

m  '' 

V 

L  sec J  j 

/  =  io-= 


(3.90) 


allows  the  GDOP  and  eondition  number  (k(T))  for  this  seenario  to  be  calculated  as  a 
function  of  tracking  time  (T). 

Figures  3.7  and  3.8  illustrate  the  behavior  of  non-dimensional  GDOP  in  this 
simple  scenario  as  time  progresses.  The  amount  of  time  a  landmark  is  tracked  is  defined 
to  be  dwell  time. 


Figure  3.7:  GDOP  vs  dwell  time  in  the  non-dimensional  case  study. 
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Figure  3,8:  Observability  grammian  matrix  condition  number  vs  dwell  time  in  tbe 

non-dimensional  case  study. 


Figures  3.7  and  3.8  illustrate  the  benefit  of  tracking  features  for  long  periods  of 
time.  A  long  dwell  time  allows  the  aircraft  to  traverse  through  space,  and  develop  larger 
observation  or  tracking  angles  on  tracked  landmarks.  The  nature  of  these  plots  show  the 
great  benefit  of  the  first  45  degrees  of  tracking  angle  (occurring  at  time  =  1  for  this  non- 
dimensionalized  case),  and  that  relatively  little  added  benefit  is  gained  past  this  point. 
The  condition  number  begins  to  rise  again  after  approximately  2.3,  but  this  is  attributed  to 
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the  tracked  landmarks  approaching  the  co-linear  condition  as  they  move  to  the  horizon. 
In  this  case,  the  geometry  effects  overcome  the  benefit  of  dwell,  and  the  matrix 
approaches  a  singular  condition. 

In  their  work,  George  and  Sukkarieh  achieved  reasonable  navigation  performance 
using  a  SLAM  algorithm  [5].  They  leveraged  dwell  time  and  a  fast  camera  rate  to 
converge  on  a  solution.  The  test  did,  however,  revisit  the  same  environment,  spiraling 
around  a  set  of  features  with  a  side  looking  camera.  Looking  into  the  turn  naturally 
increased  dwell  time.  If  this  type  of  flight  profile  meets  mission  needs,  then  performance 
can  be  greatly  increased.  However,  the  dwell  time  on  landmarks  tracked  when  traveling 
long  distances  is  driven  by  camera  look  angle  and  optical  resolution.  Performance  is  then 
affected  by  GDOP  as  landmarks  move  to  the  horizon.  A  balance  must  be  made  to 
optimize  all  variables.  In  either  case,  it  is  evident  that  tracking  a  landmark  over  time  is 
beneficial,  but  there  are  other  factors  to  consider. 

3. 3. 1.2  V  over  h  Influence  on  Aiding  Strength.  V  over  h  stands  out  as  the  key 
aircraft  controlled  parameter  (in  the  observability  grammian  matrix)  affecting  aiding 
quality.  Landmark  relative  lateral  displacement  will  change  as  a  matter  of  course  as 
landmarks  traverse  the  camera  field-of-view.  The  velocity-to-height  ratio,  however,  is 
directly  established  by  the  flight  path.  The  observability  grammian  matrix  is  a  nine¬ 
dimensional  function  making  a  concise  analysis  very  difficult.  One  dimensional 
measures  of  GDOP  and  matrix  condition  number  give  some  insight,  but  sweeping 
generalizations  cannot  be  made.  The  V  over  h  investigation  is  even  more  complex  as 
varying  velocity,  height  or  dwell  time,  ultimately  affects  the  geometry.  No  true  single 
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variable  isolation  can  be  made.  In  any  case,  this  section  will  examine  the  non- 
dimensional  scenario  in  an  attempt  to  gain  some  insight. 

Consider  the  non-dimensional  case  describe  above,  but  allow  velocity  to  vary, 
adjusting  the  V  over  h  ratio.  Because  dwell  time  and  tracking  angle  are  related  by 
velocity,  two  conditions  are  considered.  In  the  first,  the  dwell  time  is  held  constant  as 
velocity  {V over  h)  is  varied.  This  leads  to  a  tracking  angle  that  increases  with  velocity 
(V  over  h).  In  the  second,  the  total  tracking  angle  is  made  constant  by  varying  the  dwell 
time  to  achieve  the  same  distance  traveled.  In  this  case,  one  landmark  starts  45  degrees 
in  front  and  finishes  directly  below,  while  the  other  starts  directly  below  and  finishes  45 
degrees  behind.  The  constant  dwell  time  case  is  important  because  time  between  images 
(measurements)  is  constant.  Since  the  Kalman  filter  aims  to  limit  INS  error  growth  that 
accumulates  between  measurements,  it  is  important  to  examine  the  ability  of  the  system 
to  control  this  growth  effectively  over  constant  time  intervals.  These  intervals  will  of 
course  vary  with  application.  The  constant  tracking  angle  case  is  important  to  examine  as 
it  eliminates  the  geometry  from  the  equation,  leaving  the  effect  of  dynamics. 

The  effect  of  scalability  cannot  be  ignored.  It  is  also  illustrated  for  each  of  the 
conditions  describe  above.  The  height  is  varied  about  the  nominal  height  (ho)  for  three 
different  cases,  but  angular  geometry  is  held  constant.  Figure  3.9  illustrates  the  effect 
varying  V  over  h  has  on  observability  grammian  matrix  condition  number. 
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Figure  3,9:  Observability  grammian  matrix  condition  number  as  a  function  of  V 
over  h.  For  a  non-dimensionalized  case,  as  altitude  (b)  varies  about  a  nominal 
beigbt  (bo). 
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For  the  constant  dwell  time  case,  lower  V  over  h  ratios  yield  lower  condition 
numbers,  and  thus  more  evenly  distributed  aiding  (among  all  estimated  states).  A  ratio  of 
approximately  1  appears  to  be  a  knee  in  the  curve  before  the  matrix  condition  number 
grows  very  large.  This  is  due  to  the  fact  that,  for  the  non-dimensionalized  dwell  time  of 
1 ,  both  landmarks  remain  within  45  degrees  of  the  nadir  when  the  V  over  h  ratio  is  less 
than  1 .  The  dwell  time  study  above  showed  that  tracking  angles  up  to  45  degrees  provide 
the  best  aiding.  For  V  over  h  ratios  greater  than  1,  the  landmarks  have  passed  underneath 
and  proceeded  towards  the  horizon.  In  this  case,  the  geometry  has  drastically  changed 
and  the  landmarks  approach  a  co-linear  condition.  This  is  the  reason  condition  number 
grows,  not  V  over  h  ratio  itself.  When  the  landmarks  remain  inside  45  degrees  of  nadir,  V 
over  h  has  little  to  no  effect  on  grammian  matrix  condition  number  or  the  distribution  of 
aiding  among  the  navigation  state  variables. 

Notably,  a  lower  altitude  (h=ho/2)  provides  better  aiding  than  a  higher  one 
(h=ho*2),  despite  the  same  V  over  h  ratio  and  geometry  (absolute  altitude  drives  the 
magnitude  of  the  condition  number,  while  V  over  h  drives  the  shape  of  the  graph).  This 
speaks  to  the  scalability  problem  faced  by  bearings-only  measurements.  The  angular 
resolution  of  a  camera  drives  its  ability  to  discern  small  displacements  from  far  away,  and 
thus  positional  estimation  suffers.  This  study  implies  that  flying  slow  and  low  (flying 
lower  for  a  given  V  over  h  ratio)  provides  the  strongest  and  most  even  aiding.  This  is 
supported  by  Veth’s  work  on  indoor  navigation  [26]. 

In  the  constant  track  angle  case,  the  landmarks  all  remain  inside  45  degrees  of 
nadir,  and  each  experience  a  45  degree  tracking  angle.  The  dwell  time  however  varies 
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with  V  over  h.  Figure  3.9  implies  a  sweet  spot  somewhere  in  the  middle.  The  V  over  h 
ratio  faetors  into  the  observability  grammian  in  many  plaees.  Too  large  or  small  a  value 
will  naturally  increase  the  condition  number  of  the  matrix.  This  finding  merely  indicates 
that  the  aiding  will  not  be  evenly  distributed.  It  is  proposed  that  landmarks  which  have 
low  angular  rates  (or  effective  V  over  h)  provide  good  attitude  aiding,  but  poor  positional 
aiding.  It  is  also  proposed  that  landmarks  which  have  the  highest  angular  rates  (or 
effective  V  over  h)  provide  the  best  positional  aiding  and  poorest  attitude  aiding.  This 
proposition  is  supported  by  the  fact  that  a  star  tracker  (stars  are  effectively  all  on  the 
horizon  because  none  exhibit  angular  rates  due  to  aircraft  translation)  provides  excellent 
attitude  aiding,  but  no  positional  aiding.  The  fact  that  Figure  3.9  illustrates  a  non- 
dimensional  case  and  the  sweet  spot  varies  with  absolute  height  means  that  predicting  the 
sweet  spot  is  very  difficult.  An  aiding  strategy  that  maximizes  V  over  h  span  will 
increase  the  likelihood  that  the  sweet  spot  is  captured.  Also,  if  in  fact  the  two  ends  of  the 
spectrum  represent  good  attitude  aiding  and  good  positional  aiding  respectively,  then 
capturing  both  ends  will  complement  each  other.  Chapter  4  will  experimentally  evaluate 
the  effect  of  V over  h  and  attempt  to  validate  these  propositions. 

A  low  V  over  h  ratio  at  low  altitude  appears  to  be  the  most  favorable  condition  to 
provide  evenly  distributed  aiding  to  the  whole  navigation  state.  The  added  benefit  of 
increased  attitude  aiding  is  felt  throughout  the  system,  as  a  good  attitude  estimate  is 
critical  to  resolve  the  rest  of  the  navigation  state.  This  proposal  is  supported  by  the  work 
of  George  and  Sukkarieh  who  flew  a  small  UAV  at  approximately  125  meters  altitude 
and  at  slow  speeds,  looking  sideways  (into  the  turn)  while  orbiting  a  target  area.  Altitude 
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and  V  over  h  were  inherently  low,  and  they  achieved  reasonable  performance  [5]. 
However,  this  condition  may  not  be  attainable  for  many  applications  which  travel  in  a 
straight  line. 

The  full  aiding  picture  cannot  be  seen  until  GDOP  is  examined.  Consider  the 
non-dimensional  scenario  from  a  GDOP  perspective;  see  Figure  3.10.  Again,  the 
constant  dwell  time  and  tracking  angle  are  examined  as  well  as  scalability. 

In  the  constant  dwell  time  case,  there  appears  to  be  a  slight  advantage  to  a  F  over 
h  ratio  between  1  and  10,  despite  the  absolute  altitude.  In  this  region,  the  landmarks  have 
exited  the  45  degree  cone  about  nadir.  The  condition  number  has  increased,  but  the 
GDOP  has  decreased.  This  finding  is  not  intuitive,  but  suggests  that  perhaps  the  higher 
tracking  angle  (for  the  given  dwell  time)  provides  an  overall  benefit  that  overcomes  the 
effects  that  penalize  condition  number.  Thus,  there  is  yet  another  sweet  spot,  but  this 
one  at  a  higher  V  over  h  than  the  associated  sweet  spot  described  above.  This  suggests 
that  an  aiding  strategy  that  captures  higher  V  over  h  ratios  could  take  advantage  of  an 
improved  GDOP. 

Once  again,  the  scalability  principle  shows  that  absolute  altitude  significantly 
affects  GDOP  across  the  board.  Lower  absolute  altitudes  reduce  precision  dilution,  and 
thus  improve  aiding  action. 


133 


Vtrvar  h 


Q. 

o 

a 

(9 


Constant  Track  Angle 


;:  =  :  =  U*=:UM  =  =i=5U5=iU  =  =  =  :=U  =  "U=^  =  ^  =  *=^*5^======*  =  =  =^=UUn 


. . 


i  " r i’i' i ii  r . r  ' ; " m t n i  " "/ . 

ivTT'ii" 


1  ;  ; 

1. j.i.Lij j... 
!iiSiEEEBEiEE;33iii 


:  :  :  ; : 

- 1  1 - » ■  ■  r  “I*  T  "f «— 

;.II - F  I. I.C 

:r--;*"j---r-rTTTTr 

Lj.Lll.»-t..J...  j..  j..LJ.i.i.L 

. -I--- 

. 

:  :  :  :  ;;;;;: 

i  :  :  :  :  :  :  : :  :  :  :  : 

:c  up:  z.c-Z  :  =  p=  =  =  =c  =  =c:c:cp  =  czz::::::3  =  =  :p=  =  p==c=-:z=>=c 


Id*  . 
in' 


> _ >_ 

:"r‘r 

_l.  A 

1  i  i 

1  1  1  1  !  i  !  i  i  1  i  i  i  !  !  I  i  i  1  1 

1  1 

1  1  1 

in 


Ilf 

Vovwh 


in 


Ilf 


Figure  3,10:  Geometric  Dilution  of  Precision  as  a  function  of  V  over  h.  For  a  non- 
dimensionalized  case,  as  altitude  (h)  varies  about  a  nominal  height  (ho). 
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In  the  constant  track  angle  case,  a  lower  V  over  h  definitively  improves  GDOP. 
Since  the  aircraft-to-landmark  geometry  dependence  is  removed,  it  isn’t  readily  apparent 
why  this  is  the  case.  If  the  discrete  case  is  considered,  a  lower  V  over  h  allows  more 
measurements  in  the  same  tracking  angle.  More  measurements  allow  the  corrupting 
errors  to  be  averaged  out  more  precisely.  If  the  limit  is  taken  such  that  a  continuous  set 
of  measurements  is  available,  then  the  benefit  of  a  lower  V  over  h  is  understandable.  The 
dynamics  of  the  inertial  system  cannot  be  ignored  either.  In  any  case,  this  finding 
suggests  that  an  aiding  strategy  that  captures  lower  V  over  h  ratios  could  take  advantage 
of  an  improved  GDOP. 

So  far  there  has  been  evidence  that  capturing  both  high  and  low  V  over  h  ratios 
provide  benefit.  This  cannot  be  accomplished  with  a  single  landmark,  but  can  be  done  by 
tracking  many  different  landmarks  simultaneously.  V  over  h  ultimately  drives  aiding 
strength  and  distribution,  and  will  be  evaluated  experimentally  in  Chapter  4. 

3. 3. 1.3  Look  Angle  Influence  on  Aiding  Strength.  The  question  then  arises, 
what  V  over  h  and  what  altitude  should  be  flown  for  a  given  application  (aircraft  and 
mission).  This  decision,  of  course,  will  be  influenced  by  the  performance  of  the  aircraft 
and  its  mission  requirements,  but  V  over  h  can  be  affected  another  way.  Given  an  aircraft 
flying  over  flat  terrain,  the  V  over  h  ratio  described  only  applies  at  nadir.  In  actuality,  V 
over  h  should  be  considered  a  measure  of  apparent  angular  rate,  which  varies  throughout 
the  scene.  Objects  on  the  horizon  show  zero  angular  rate  (for  straight  and  level  flight), 
while  V  over  h  is  highest  at  nadir.  The  benefit  of  a  lower  V  over  h  is  shown  in  Figures 
3.9  and  3.10.  The  portion  of  the  scene  near  the  horizon  naturally  exhibits  low  V  over  h. 
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The  portion  of  the  seene  near  nadir  naturally  exhibits  higher  V  over  h,  but  doesn’t 
neeessarily  approaeh  the  eritieal  and  unknown  sweet  spots.  V  over  h  ean  be,  to  an  extent, 
varied  by  observing  different  portions  of  a  seene. 

Suppose  that  the  entire  seene  exhibits  a  V  over  h  less  than  1  see'\  This  is  optimal 
for  even  distribution  of  aiding  (for  eonstant  time  intervals),  but  ean’t  be  generally 
guaranteed.  However,  there  exists  a  portion  of  every  seene,  between  nadir  and  the 
horizon,  where  V  over  h  is  less  than  this  threshold.  It  must  be  stressed  that  eondition 
number  suffers  if  the  lowest  V  over  h  is  eaused  by  traeking  landmarks  on  the  horizon. 
These  landmarks  will  never  generate  large  traeking  angles.  With  a  wide  enough  lens,  the 
eamera  FOV  should  strive  to  eapture  portions  near  the  horizon  for  even  aiding  strength  to 
all  navigation  states  (over  eonstant  time  intervals)  in  order  to  ensure  that  all  navigation 
state  errors  are  eonstrained.  However,  due  to  the  naturally  low  traeking  angles  of  these 
landmarks,  a  eomplementary  approaeh  that  traeks  landmarks  over  large  angles  must  be 
aeeomplished. 

If  a  landmark  position  requires  triangulation,  a  lower  GDOP  is  more  favorable. 
This  was  shown  to  happen  between  a  V  over  h  and  10  see'^  (for  the  non-dimensional 
ease).  This  eondition  eannot  be  guaranteed  in  any  seene,  but  there  is  no  penalty  for 
trying.  Given  eonventional  aireraft  performanee,  this  eondition  would  rarely  exist 
anywhere  other  than  at  nadir.  Also,  it  would  be  rare  for  a  eonventional  aireraft  to  exeeed 
a  V  over  h  of  3-5  see'^  at  nadir.  Thus,  to  ensure  the  maximum  benefit  of  a  low  GDOP,  a 
eamera  FOV  should  strive  to  eapture  nadir.  Sinee  a  low  V  over  h  ean  always  be 
guaranteed,  and  a  high  V  over  h  eannot,  eapturing  nadir  should  outweigh  eapturing  the 
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horizon  if  camera  field-of-view  is  limited,  espeeially  sinee  landmarks  traeked  on  the 
horizon  require  another  to  be  orthogonally  traeked  at  high  V  over  h. 

The  purposed  sweet  spots  in  the  V  over  h  speetrum  eannot  be  praetieally  predieted 
or  eontrolled.  Despite  this  nature,  it  has  already  been  shown  that  eapturing  both  high  and 
low  V  over  h  potions  of  the  seene  is  benefieial.  Fortunately  the  sweet  spots  reside 
between  the  high  and  low  limits  demonstrated  in  the  non-dimensional  ease.  An  aiding 
strategy  that  eaptures  the  horizon  and  nadir  will  maximize  the  potential  to  eapture  the  V 
over  h  sweet  spots  deseribed  here. 

Sinee  landmarks  generally  exist  below  the  horizon,  a  180  degree  field-of-view 
eamera  lens  eould  reasonably  view  the  entire  seene  presented  to  an  aireraft.  This, 
however,  ereates  a  eomplex  ealibration  problem.  A  90  degree  field  of  view  is  a  praetieal 
eompromise  between  pin-hole  eamera  model  ealibration,  and  eapturing  enough  of  the 
seene.  This  would  also  ensure  eapturing  nadir  and  one  horizon,  but  whieh  horizon  is 
optimal?  Humans  look  forward,  eapturing  the  forward  horizon,  but  this  is  beeause 
humans  use  their  eyes  for  navigation  and  guidanee.  From  an  aviation  navigation 
standpoint,  guidanee  ean  be  aeeomplished  by  steering  to  inertial  eoordinates,  freeing  the 
eamera  to  look  anywhere  (if  dedieated  to  the  navigation  task). 

An  EKF  is  an  iterative  proeess,  and  the  element  of  time  ordering  plays  an 
important  role.  The  method  by  whieh  a  landmark  enters  and  exits  the  seene  affeets  the 
resultant  aiding.  If  a  landmark’s  position  is  well  surveyed,  then,  arguably,  order  doesn’t 
matter.  One  would  want  simply  to  traek  it  for  as  long  as  possible,  or  dwell  on  it,  with  the 
lowest  possible  observability  grammian  eondition  number  and  GDOP  to  ensure  the  most 


137 


optimal  aiding.  Suppose,  however,  that  the  landmark  position  is  not  known.  It  would 
have  to  be  loeated  by  triangulation  before  dwelling  on  it  would  provide  any  benefit.  This 
suggests  that  the  eamera  should  be  positioned  to  observe  landmarks  in  the  higher  V  over 
h  portion  of  the  seene  before  dwelling  in  the  lower  V  over  h  portion  of  the  seene.  For  a 
90  degree  field-of-view,  this  means  pointing  the  eamera  baekwards  at  a  45  degree  angle. 
Figure  3.11  illustrates  this  prineiple. 


Figure  3.11:  Rear  facing  camera  geometric  advantage. 

A  rearward  looking  eamera  allows  landmarks  to  be  triangulated  before  they  are 
traeked,  thus  maximizing  useful  dwell  time.  This  assertion  is  eounter-intuitive,  but 
insightful  and  unique  to  this  researeh. 

If  time  were  not  a  faetor,  i.e.,  an  offline  optimal  smoother  or  bateh  estimator  was 
used,  order  of  flow  would  not  matter.  However,  it  was  shown  in  Figure  3.7  that  dwell 
time  affeets  eondition  number.  Looking  baekwards  maximizes  useful  dwell  time  on 
features  that  have  been  first  triangulated.  Looking  forward,  the  system  dwells  on 
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landmarks  before  they  have  been  triangulated  well  (the  best  GDOP  oeeurs  just  before 
landmarks  leave  the  seene).  This  is  of  eourse  a  simplifieation  of  a  eontinuous  proeess, 
but  the  prineiples  remain.  An  experimental  evaluation  of  eamera  look  angle  is 
aeeomplished  in  Chapter  4,  supporting  this  assertion. 

3. 3. 1.4  Scene  Geometry  Influence  on  Aiding  Strength.  From  a  striet 
observability  standpoint,  one  needs  only  to  traek  two  surveyed  landmarks.  Matrix 
eondition  and  GDOP,  however,  depend  on  geometry.  Traeking  many  randomly  placed 
landmarks  inerease  the  probability  of  attaining  good  geometry  (a  low  eondition  number 
and  good  GDOP)  at  all  times.  The  observability  grammian,  eondition  number,  and 
GDOP  ealeulations  are  highly  non-linear,  eomplex,  time-variant,  and  thus  diffieult  to 
prediet.  A  single  study  of  the  effeets  of  seene  geometry  provides  little  predietive  value 
for  deeiding  whieh  geometry  would  best  suit  the  myriad  of  seenarios  an  aireraft  eould 
faee.  In  praetiee,  one  would  benefit  from  an  over-determined  system,  by  traeking  as 
many  landmarks  as  possible,  providing  a  varied  set  of  geometry  at  all  times.  Using 
statistieal  brute  foree,  randomly  seleeted  landmarks  spanning  the  seene  give  the  most 
robust  strategy  to  ensure  a  low  GDOP  and  eondition  number  at  all  times. 

3.3.2  Alternative  Measurements  to  Augment  Observability.  Reeonsider  the 
single  landmark  traeking  ease.  It  is  an  under-determined  problem,  resulting  in  a  less- 
than-full-rank  observability  grammian  matrix.  The  question  is  posed,  what,  if  any,  other 
passive  measurements  may  be  made  to  aehieve  full  rank?  This  investigation  is  pivotal  in 
the  development  of  an  aiding  strategy  for  traeking  landmarks  of  opportunity. 
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An  inspection  of  the  observability  grammian’s  RREF  in  Equation  (3.72)  reveals 
that  measuring  h  eliminates  ambiguities  between  height  and  velocity  (allowing  the  hiv 
dimension  to  be  resolved).  Both  position  and  velocity  would  benefit  since  this  term 
appears  in  row  3  and  6,  suggesting  a  potential  rank  increase  of  2.  Additionally,  the 
specific  force  vector  components  (f^y^)  are  factored  with  A^^  and  v,  requiring  a 

measurement  to  resolve.  In  level  flight  are  essentially  0,  leaving  only  v  and  the 
horizontal  EOS  components  (A^^)  in  the  last  column.  The  most  immediately  evident 
solution  is  to  measure  heading  (^)  in  order  to  resolve  directional  ambiguities.  This 
suggests  that,  in  lieu  of  tracking  additional  landmarks,  full  observability  may  be 
achievable  with  passive  measurements  that  are  readily  available  on  most  aircraft;  altitude 
via  an  altimeter,  and  heading  via  a  compass. 

Taking  an  altitude  measurement  results  in  augmenting  the  observation  matrix  as 

such: 

H'  (x,(0)  = 


H  (x,(0) 

[0  0  1  0  0  0  0  0  0] 


(3.91) 


where  H  (Xp(t))is  defined  in  Equation  (3.59).  Recalculating  W(t)  reveals  that  the 
simple  addition  of  an  altitude  measurement  increases  the  rank  of  W  (t)  from  6  to  8.  The 
RREF  becomes: 
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1  0  0  0  0  0  0  0  -A^ 
01000000 
00100000  0 
00010000  0 
00001000  V 
00000100  0 
00000010  0 
00000001  0 
00000000  0 


Notably,  when  the  un- accelerated  flight  restriction  is  removed,  full  rank  is  achieved. 
Maneuvering  has  consistently  been  shown  to  help  aiding. 

Likewise,  taking  a  heading  measurement  results  in  augmenting  the  observation 
matrix  as  such: 


H'  (x,(0) 


H  (x,(0) 

[0  0000000  1] 


(3.93) 


Recalculating  W  (t)  reveals  that  the  simple  addition  of  a  heading  measurement  increases 
the  rank  of  W  (t)  from  6  to  7.  The  RREF  becomes: 
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0 

0 

1 

0 
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(3.94) 


Notably,  when  the  un-aeeelerated  flight  restrietion  is  removed,  a  rank  of  8  is  aehieved. 
Maneuvering  has  eonsistently  been  shown  to  help  aiding. 

Lastly,  taking  both  a  heading  and  altitude  measurement  results  in  augmenting  the 
observation  matrix  as  sueh: 


H'  (x,(0) 


H  (x,(0) 

[0  0  1  0  0  0  0  0  0] 
[0  0000000  1] 


(3.95) 


and  produces  a  full  rank  observability  grammian  matrix. 
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Notably,  measuring  either  or  A^,  (aireraft  position  states)  yield  the  same  result 

as  measuring  h ,  but  are  not  measurements  that  ean  be  made  passively  like  altitude. 
Single  ehannel  inertial  veloeity  measurements  inerease  rank  by  1,  but  eannot  be  measured 
by  typical  (independent)  passive  sensors.  Additionally,  they  have  no  effect  on  rank  when 
altitude  is  simultaneously  being  measured.  Roll  or  pitch  measurements  have  the  same 
effect  as  heading,  but  are  again  not  available  through  (independent)  passive  sensors. 
Lastly,  only  heading  or  roll  measurements  solve  the  observability  problem  caused  by  the 
zero-g  condition,  restoring  full  rank. 

In  summary,  independent  measurements  of  the  heading  (^),  the  altitude  {h),  and 
bearings  measurements  of  a  single  known  landmark  make  it  possible  to  update  the 
complete  nav-state.  Atmospheric  pressure  bias  and  magnetic  variation  would  need  to  be 
considered,  but  change  slowly  and  should  provide  little  challenge.  This  is  an  important 
finding  as  the  additional  measurements  of  ^ will  be  further  discussed  in  the  case  when 
landmark  position  is  not  known  independently,  and  full  observability  cannot  be  achieved 
through  landmark  bearing  measurements  alone. 

3.3.3  Estimated  Landmark  Position  Tracking  Observability.  Tracking  surveyed 
landmarks  (without  augmenting  the  navigation  state  vector  to  estimate  their  position)  is  a 
relatively  trivial  task  and  has  been  demonstrated  before  [8].  When  landmark  positions 
must  be  estimated  (as  proposed  in  SLAM  and  SLAAMR),  the  navigation  state  dimension 
grows  and  the  observability  grammian  changes.  As  described  in  Section  3.2.6,  when 
tracking  N  landmarks  of  unknown  position  or  landmarks  of  opportunity  (LOO),  the 
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navigation  and  error  states  are  augmented,  and  the  observation  matrix  H(x^(t))  is 
augmented  with  two  additional  rows  for  eaeh  of  the  N  traeked  LOO: 


H(x,(0)  = 


Hn(Xc(0). 


H,(x,(0)  = 


/ 

■-a/c:e,c: 

:  0^,3 

s  A/c: 

(e,A/(C:e,; 

)x-e,AT(C>,): 

(a/C>3)" 

_+a/c:e,c: 

: 

i  A/c:i 

)x-e3A/(C:e,) 

'lx(3.(i-l)) 


'lx(3.(i-l)) 


:  +a/c:e,c:  :  o, 


x(3.{7V-0) 


-a/c:e,c: 


0 


lx(3.(V-0) 


(3.96) 


When  the  observability  grammian  matrix  is  ealeulated  with  the  new  observation 
matrix  and  state  veetors,  it  takes  a  similar  form  as  in  the  known  traeking  ease.  The  inner 
9x9  matrix  is  identieal 


Wn(0,x9=W,(0 


(3.97) 


where  W^(t)  is  the  observability  grammian  matrix  for  N  estimated  landmark  traeks  and 
is  the  observability  grammian  matrix  from  the  N  known  landmark  traeking  ease  as 
defined  in  Equation  (3.76). 
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Additionally,  the  summation  property  from  Equation  (3.76)  remains  true. 


w„(t)=£w;(t) 


(3.98) 


and  W.  (t)  is  defined 


w;(t) 


w, 

0(3.(,-l))x9 

“^)(1:9,1:3) 

09x(3.(N-))) 

*(3.(,-l))x9 

®(3.(i-l))x(3.(i-l)) 

^3x3 

^3x3 

w 

**/(l:3,l:9) 

^3x3 

^i(l:3,l:3) 

^3x3 

'(3-(N-/))x9 

^3x3 

^3x3 

0(3.(N-,))x(3(N-,)) 

(3.99) 


where  0  „  is  a  m-hy-n  matrix  of  zeros,  W.  is  short  hand  for  W  (t)  and  W..  ,  .  is  a 

subset  of  W.  defined  by  the  j  through  row  and  m  through  n*  eolumn.  It  stands  to 
reason  that  a  similar  extension  is  made  for  the  RREF  of  W^(t)  . 

Two  questions  arise  and  must  be  addressed.  First,  ean  full  observability 
grammian  matrix  rank  be  obtained  when  no  survey  is  provided?  Second,  does  the 
increased  navigation  state  dimension  affect  observability  when  landmark  positions  are 
surveyed  independently,  but  inserted  as  “good”  estimates?  The  answer  to  the  first 
question  is  no,  and  the  second  is  yes,  but  for  all  practical  purposes,  no.  The  development 
follows. 

3. 3. 3.1  Landmark  of  Opportunity  Tracking.  Again,  consider  the  case  of  un¬ 
accelerated  flight  with  two  tracked  landmarks,  with  positions  that  are  now  estimated. 
Call  these  landmarks  of  opportunity  (EOO).  W'(t)  becomes  a  15x15  matrix  with  rank 
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10.  Understandably,  full  rank  cannot  be  achieved  by  measuring  angular  differences 
between  two  positions,  both  of  which  are  unknown.  In  fact,  for  the  specific  case  of 
level,  un-accelerated  flight  over  flat  terrain,  rank  trails  state  dimension  by  a  constant 
value,  5,  as  the  number  of  tracked  landmarks  increases.  State  dimension  grows 

linearly  with  number  of  tracked  landmarks  as  follows 

N.=N,„+3-N,, 

Rank(WN(0)  =  N^-5  (3.100) 

where  is  the  length  of  the  basic  navigation  state  (9  or  15,  depending  on  inclusion  of 

sensor  bias  states).  This  constant  offset  suggests  that  tracking  many  landmarks  may 
eventually  dilute  the  effect  of  the  underdetermined  set  of  measurement  equations.  The 
dimensions  along  which  observability  is  not  obtained  are  not  apparent,  but  their  number 
does  not  grow  with  added  landmark  tracks.  It  cannot  be  said  that  tracking  more  LOO 
will  alter  the  nature  of  the  unobserved  dimensions,  but  there  is  no  observability  penalty 
for  tracking  many.  It  is  proposed  that  tracking  many  LOO  may  dilute  the  effect  of 
deficient  rank  with  no  penalty. 

This  proposition  is  key  to  the  design  of  the  SLAAMR  algorithm,  as  it  incorporates 
many  simultaneous  tracks.  The  effects  of  tracking  varying  numbers  of  LOO  will  be 
evaluated  experimentally  in  Chapter  4.  Tracking  more  landmarks  increases  state 
dimension  linearly,  but  computation  speed  geometrically.  For  the  SLAAMR  algorithm, 
implemented  in  Matlab  ,  using  ten  tracks  was  a  practical  compromise  between  speed  and 
performance.  Ten  tracks  were  sufficiently  many,  yet  allowed  real-time  computation. 
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Another  important  finding  is  that,  if  both  tracked  landmarks  fall  along  the  flight 
path,  rank  drops  from  10  to  9.  This  is  an  important  distinction  from  the  known  landmark 
tracking  case  in  which  lateral  offset  ( 0)  did  not  affect  rank.  This  speaks  to  the 

importance  of  varied  landmark  geometry.  Along  with  tracking  many  landmarks, 
SLAAMR  ensures  to  maximize  lateral  separation  between  chosen  landmarks.  To  ensure 
a  robust  solution  allowing  for  rolls  and  turns,  new  landmarks  are  chosen  to  track  such  that 
the  entire  population  of  tracked  features  spans  the  camera  frame.  This  allows  rolls  and 
turns  in  any  direction  without  dropping  a  significant  portion  out  of  the  field  of  view. 
SLAAMR  accomplishes  this  by  weighting  new  feature  selection  based  on  distance  from 
each  other,  frame  edges,  open  space  in  the  image,  etc.  This  is  an  ad  hoc  technique  and 
designed  with  engineering  judgment. 

When  the  additional  independent  measurements  of  altitude  and  heading  are 
applied  as  described  in  Section  3.3.2  to  the  case  in  which  two  unknown  landmarks  are 
tracked,  rank  increases  from  10  to  12  (full  rank  being  15);  each  measurement  contributing 
one  order  of  rank.  In  fact,  rank  now  trails  state  dimension  by  only  3  as  the  number  of 
tracked  landmarks  increases.  This  holds  true  when  the  level  flight  restriction  is  removed. 
In  the  known  landmark  position  tracking  case,  heading  and  altitude  measurements 
enabled  full  observability  when  tracking  a  single  landmark.  In  the  unknown  case,  full 
rank  can  never  be  achieved.  Nonetheless,  observability  increases,  and  the  effects  of  these 
measurements  is  evaluated  experimentally  in  Chapter  4. 

3. 3. 3. 2  Using  DIED  to  Constrain  Errors  Further.  Digital  Terrain  Elevation 
Data  (DTED)  is  a  satellite-based  survey  of  the  Earth’s  surface.  It  can  be  used  to  resolve 
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uncertainty  in  a  LOO’s  vertical  position.  A  direct  measurement  eannot  be  taken,  but 
interseeting  the  3-dimentional  DIED  map  with  the  LOO’s  image  projeetion  provides  a 
good  estimate  of  elevation.  With  perfeet  knowledge  of  aireraft  position  and  attitude,  the 
errors  in  a  DTED-based  LOO  elevation  estimate  are  dependent  on  the  fidelity  of  the 
DEED  map  itself  However,  if  the  aireraft  position  and  attitude  estimate  used  to  interseet 
the  map  are  errant,  the  wrong  portion  of  the  map  will  be  interseeted,  and  the  estimate  will 
be  in  error.  Areas  of  fiat  terrain  are  naturally  less  prone  to  this  souree  of  error,  while 
mountainous  regions  are  more  prone.  In  either  ease,  without  perfect  knowledge  of 
aireraft  navigation  state,  DEED  interseetion  errors  are  not  independent  of  the  navigation 
state  errors,  nor  are  they  zero-mean  or  Gaussian. 

Consider,  however,  that  perfeet  navigation  state  knowledge  was  obtainable,  and 
two  LOO  were  being  tracked  and  their  positions  estimated.  Ehe  observability 
eontribution  of  ineorporating  DEED  interseetions  should  be  studied.  Ehe  DEED 
interseetion  produees  a  3-dimensional  position  estimate,  but  only  the  vertieal  element  is 
independent  of  the  navigation  state  estimates.  If  DEED  interseetions  eould  be  eonsidered 
independent  and  direet  measurements  of  landmark  vertieal  position,  the  observation 
matrix  would  be  augmented  as  sueh: 


H'  (x,(0) 


H  (x,(0) 

[0  0  •••  0  0  1  0  0  0] 

[0  0  •••  0  0  0  0  0  1] 


(3.101) 
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where  the  third  (vertieal)  position  state  of  both  LOO  are  direetly  measured.  The  resultant 
observability  grammian  matrix  rank  inereases  by  an  order  of  1,  no  matter  how  many 
LOO  are  being  traeked.  This  holds  true  whether  one  LOO  vertieal  position  is  estimated 
with  DTED  or  all  are. 

This  behavior  also  extends  to  the  ease  in  whieh  heading  and  aireraft  altitude  are 
measured,  eausing  rank  to  trail  state  dimension  by  only  2  as  the  number  of  tracked 
landmarks  increases.  This  study  is  ideal,  and  not  truly  realizable,  but  the  practical  effect 
of  DTED  intersection  is  experimentally  evaluated  in  Chapter  4. 

3. 3. 3. 3  Surveyed  Landmarks  with  Augmented  State  Vector.  Consider  that  the 
tracked  landmarks  were  independently  surveyed,  but  done  so  imperfectly.  In  essence,  the 
landmark  positions  are  estimated,  but  the  uncertainty  is  small,  and  the  source  is 
independent  of  the  system  being  aided.  The  independent  position  measurement/survey  is 
applied  directly  to  the  landmark  position  states,  and  the  uncertainty  applied  to  the  error 
state  covariance  matrix.  The  state  vector  is  the  collection  of  estimated  states  augmented 
with  the  landmark’s  surveyed  position 


X 


VT>«  T 

^  nb 


P  survey 


T 


(3.102) 


and  the  associated  3-dimensional  uncertainty  is  inserted  into  the  whole  state 

covariance  matrix  (P(tJ),  with  zeros  filling  the  cross  covariance  elements: 
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/  j-\-A-.N  J+A-.N 


(3.103) 


where  P  is  a  subset  of  the  whole  eovariance  matrix  P  (t,. ) ,  and  is  a  jxk  matrix 

of  zeros. 

Beeause  the  landmarks  are  assumed  to  be  stationary,  their  positional  uneertainty 
volume  does  not  grow  with  time  (effective  portion  of  state  transition  matrix  is 

identity).  In  fact,  measurements  on  these  injected  landmarks  only  serve  to  better  the 
initial  surveyed  position  estimate.  If  the  initial  positional  uncertainty  of  the  surveyed 
landmark  is  small  enough  (relative  to  other  error  states),  the  Kalman  filter  will  treat  it  as 
if  it  is  near  truth.  Most  of  the  correction  derived  from  measuring  bearings  to  the 
landmark  will  be  distributed  to  the  navigation  states.  In  fact,  as  the  uncertainty  of  the 
surveyed  position  approaches  zero  ( 63^3 ),  the  landmark  position  states  become 

completely  independent,  and  may  be  eliminated  from  the  EKF.  This  proposition  is 
supported  by  examining  the  Kalman  filter  gain  in  two  non-dimensionalized  cases. 

Consider  an  aircraft  traveling  at  unit  velocity,  at  unit  height,  tracking  a  single 
landmark  displaced  a  unit  distance  laterally  and  longitudinally.  All  navigation  states 
have  identical,  unit  valued,  uncertainties  (P=ldentity).  The  Kalman  filter  gain  (K)  is 


K  = 


0 

028 


.13 

0 


0  0  0  0  -.13  0  .25 

-.056  0  0  0  0  .14  0 


0  -.13  0 

-.028  0  .056 
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Note  that  both  aircraft  position  (first  three  columns)  and  landmark  position  (last  three 
columns)  Kalman  filter  gain  values  are  equivalent  in  magnitude.  (Ignore  the  zero-valued 
velocity  state  gains  as  off-diagonal  cross-correlations  were  not  present  in  this  case, 
P=Identity).  Contrast  this  case  with  one  in  which  the  landmark  position  is  10  times  more 
certain: 


K  = 


0 

031 


.12 

0 


0  0  0  0  -.12  0  .25 

-.062  0  0  0  0  .16  0 


0 

-.0003 


-.0012  0 
0  .0006 


In  this  case,  the  landmark  position  gains  are  1/100*  that  of  aircraft  position.  Nearly  all  of 
the  aiding  is  distributed  to  the  navigation  states,  and  very  little  to  the  landmark  position 
states.  In  the  known  landmark  position  tracking  case,  all  aiding  is  distributed  to  the 
navigation  states  and  none  to  the  landmark  position.  As  proposed  above, 
as  63^3 ,  the  effective  state  dimension  decreases  as  the  landmark  position  states 

essentially  fall  out.  This  examination  of  the  Kalman  filter  gain  indicates  that  no  aiding 
action  is  lost  in  this  pursuit  (only  the  aiding  action  to  the  landmark  position  states  is 
affected).  This  suggests  that  there  exists  a  threshold  below  which,  a  well  surveyed 
landmark  will  cause  the  EKF  to  behave  (practically)  like  it  would  be  for  a  landmark 
position  that  is  known.  This  is  true  despite  the  fact  that  the  state  vector  (having  been 
augmented  to  estimate  landmark  positions)  dimension  exceeds  the  rank  of  the 
observability  grammian.  Due  to  the  stationary  nature  of  landmark  positions,  once  below 
this  threshold,  this  behavior  persists  as  long  as  it  is  tracked.  This  behavior  is  exhibited 
for  all  valid  realizations  of  P. 
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It  is  thus  proposed  that,  once  a  well  surveyed  landmark  is  injected,  the  algorithm 
performs  as  if  the  position  is  known  as  long  as  it’s  tracked.  From  an  observability 
standpoint,  this  aiding  benefit  must  be  examined.  Consider  the  case  in  which  two 
unknown  landmarks  are  being  tracked.  The  augmented  measurement  equation  when 
injecting  the  surveyed  position  for  both  landmarks  is 


H'  (x.(0)  = 

where  is  a  matrix  of  zeros,  and  1^,^^  is  an  identity  matrix  allowing  the  direct 

measurement  of  the  last  6  states,  the  landmark  position  estimates.  In  this  case,  full  rank 
is  achieved.  In  fact,  if  the  number  of  estimated  landmarks  is  greater  than  2,  full  rank  is 
still  achieved  by  measuring  the  position  of  just  two.  This  implies  that,  despite  many 
estimated  landmarks,  one  need  only  know  the  location  of  two,  to  have  complete 
observability.  Section  3.3.1  showed  this  to  be  true  in  the  known  landmark  case,  but  the 
extension  to  the  estimated  landmark  position  case  is  notable,  and  will  prove  fruitful  in  the 
next  section.  Granted,  this  measurement  equation  only  applies  at  the  epoch  that  the 
landmark  position  data  is  injected,  but  because  the  uncertainty  is  small  (below  an 
appropriate  threshold),  and  the  survey  source  is  independent,  the  benefit  is  felt  with  every 
measurement  afterwards. 

A  direct  landmark  position  measurement  is  not  taken  at  every  epoch,  but  the 
effective  state  dimension  is  reduced,  and  the  system  becomes,  for  all  practical  purposes. 


H  (x.(0) 

[^6x9  ^exe] 


(3.104) 
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fully  observable.  It  can  thus  be  proposed  that  no  direct  measurement  need  be  taken  at  the 
epoch  of  insertion  either.  One  need  only  insert  the  landmark’s  independently  well 
surveyed  position  and  associated  uncertainty  covariance  into  the  EKF  as  described  above. 
This  allows  the  seamless  inclusion  of  surveyed  landmarks  into  an  algorithm  designed  to 
track  and  estimate  LOO.  No  modification  to  the  algorithm  is  necessary.  The  only 
difference  between  a  LOO  and  surveyed  landmark  is  the  source  and  confidence  of  its 
position  estimate.  The  SLAAMR  algorithm  leverages  this  finding  to  allow  a  single 
process  to  include  both  types  of  landmarks  seamlessly  without  special  handling  functions. 

It  is  purposed  that,  when  using  an  LKL,  injecting  independently  surveyed 
landmarks  into  a  state  vector  comprised  of  estimated  landmark  position  states  will  exhibit 
comparable  performance  to  an  algorithm  that  assumes  perfect  knowledge  of  landmark 
position  (and  does  not  augment  the  state  vector  for  estimation).  This  is  not  to  say  that  the 
assumption  of  perfect  knowledge  implies  that  landmark  positions  are  actually  perfectly 
known.  No  survey  is  perfect,  and  uncertainty  is  inherent.  This  proposed  SLAAMR 
strategy  accounts  for  imperfect  surveys,  and  may  actually  perform  better.  A  perfectly 
known  landmark  position  also  does  not  grant  immunity  to  poor  GDOP,  as  real  cameras 
have  finite  resolution.  This  proposal  will  be  evaluated  experimentally  in  Chapter  4. 


3.4  Self-Surveying  Landmarks  of  Opportunity 

In  order  to  track  a  LOO,  it  must  be  inserted  into  the  LKL.  This  is  done  in  the 
same  manner  as  the  surveyed  landmarks,  as  discussed  above.  The  only  difference  is  the 
source  of  the  position  estimate  and  the  associated  uncertainty.  Through  the  course  of 
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tracking  a  LOO,  the  uncertainty  in  its  position  estimate  shrinks,  but  will  never  be  less 
than  that  of  the  host  aireraft.  The  SLAM  concept  asserts  that,  through  bearings-only 
measurements,  a  3 -dimensional  map  of  the  surroundings  can  be  created.  Many 
teehniques  have  been  purposed,  sueh  as  batch  estimators  and  binocular  vision.  The 
SLAAMR  algorithm  uses  the  behavior  deseribed  here.  By  simply  inserting  LOO  into  the 
EKF,  the  proeess  of  eontinual  observation,  or  tracking,  will  allow  the  landmark  estimates 
to  shrink  in  uncertainty,  and  build  a  map. 

3.4.1  Building  a  Map.  A  landmark  map  is  essentially  a  collection  of  landmark 
position  estimates,  feature  descriptors  to  allow  tracking,  and  associated  position 
uncertainties.  Suppose  aircraft  position  and  attitude  are  known  preeisely.  In  this  case, 
the  resultant  map  created  by  flying  around  a  spaee  (and  tracking  landmarks)  would  also 
be  precise.  This  ean  be  aecomplished  through  additional  GPS  aiding  of  the  INS  (either 
tightly  coupled  or  loosely  coupled)  [21].  One  need  only  augment  the  measurement 
equation  with  direet  measurements  of  the  aircraft  position  states  (loosely  coupled 
example). 


H' (x.(0) 


H  (x,(0) 

[^3x3  03xm] 


(3.105) 


where  M  is  the  length  of  the  whole  error  state.  Tightly  coupled  GPS  updates  (requiring 
estimation  and  measurement  of  satellite  vehicle  pseudo-ranges)  are  well  established, 
though  slightly  more  eomplex.  In  either  case,  this  type  of  proeedure  is  well  understood 
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and  will  not  be  discussed  further.  If  the  navigation  state  is  constrained  tightly  enough, 
the  landmarks  in  the  map  can  approach  the  threshold  where  they  would  be  considered 
well  surveyed.  The  takeaway  is  that  the  simple  augmentation  of  the  measurement 
equation  (to  allow  a  constrained  navigation  state),  enables  the  basic  SLAAMR  algorithm 
to  build  a  precise  map  of  surveyed  landmarks.  This  seamless  process  can  be  turned  on 
and  off  with  no  additional  modification  to  the  underlying  algorithm.  The  SLAAMR 
algorithm  can  transition  from  building  precise  maps,  to  using  them  for  aiding  with  the 
‘flip  of  a  switch’. 

3.4.2  Constraining  Drift  with  Brute  Force.  Consider  now,  that  some  off-board 
aiding  (like  GPS)  is  not  available.  Previously  it  was  shown  that  tracking  two  known 
landmarks  gives  full  observability.  It  was  also  suggested  that  tracking  two  well  surveyed 
landmarks  gives  the  same  practical  benefit,  despite  the  number  of  LOO  otherwise  being 
tracked  and  estimated.  Suppose  two  surveyed  landmarks  are  inserted  into  the  EKF  at  the 
beginning  of  a  flight  (a  valid  alignment  is  assumed).  It  is  not  unreasonable  to  have  a 
small  set  of  surveyed  landmarks  at  the  base  of  operations.  As  the  initial  two  landmarks 
are  tracked  over  time,  it  was  shown  that  the  aircraft’s  position  uncertainty  will  approach 
that  of  the  surveyed  landmarks.  It  was  also  shown  that  the  position  estimates  of  the 
remaining  LOO  will  approach  that  of  the  aircraft,  and  thereby  the  initial  surveyed 
landmarks  (assuming  favorable  geometry).  Eventually,  the  initial  landmarks  will  leave 
the  camera  field  of  view  and  no  longer  be  able  to  be  tracked.  However,  the  remaining 
EOO,  with  positional  uncertainties  that  have  approached  that  of  the  original  surveyed 
landmarks,  take  over;  only  two  are  required.  The  cycle  repeats  for  the  EOO  that  take  the 
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place  of  the  surveyed  landmarks  that  were  lost.  As  it  goes,  the  well  surveyed  landmarks 
constrain  the  navigation  state,  which  is  then  used  to  self-survey  new  landmarks  to  take 
their  plaee.  Figure  3.12  illustrates  this  proeess. 


Figure  3,12:  Iterative  map  building  and  aiding.  New  landmarks  are  tracked, 
refining  their  estimates  and  eventually  used  for  primary  aiding. 

In  essence,  if  the  optics  and  inertial  data  were  perfect,  and  the  geometry  just  right, 
the  system  could  continue  indefinitely  without  drifting.  The  fact  of  the  matter  is,  optics 
are  flawed,  an  INS  drifts,  tracking  is  imperfect,  and  geometry  is  at  the  mercy  of  the  flight 
path  and  geography. 

The  brute  force  approach  (to  constraining  navigation  drift)  is  to  maximize 
likelihood  of  having  good  geometry  and  well  surveyed  landmarks  by  simultaneously 
tracking  as  many  LOO,  spread  throughout  the  image,  as  computational  power  will  allow. 
This  approach  statistically  hedges  the  bet  that  there  will  always  be  at  least  two  ‘well 
surveyed  features’  available  to  limit  drift.  Realistically,  errors  in  optics  and  tracking 
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guarantee  a  dilution  of  preeision  through  this  eyele,  allowing  drift  to  oeeur.  The  amount 
of  drift  is  then  a  produet  of  these  faetors.  Chapter  4  will  experimentally  demonstrate  that 
preeise  opties  and  this  brute  foree  approaeh  ean  signifieantly  reduee  drift,  without  the  aid 
of  GPS  or  pre-surveyed  maps.  This  is  the  key  differenee  between  the  SLAAMR 
algorithm  and  previous  work.  SLAAMR  is  a  single  algorithm/proeess  that  ean 
seamlessly  transition  from  building  preeise  maps  with  the  aid  of  GPS,  to  using  these 
maps  to  provide  drift- free  navigation,  to  navigating  eompletely  free  of  external  help. 


3.5  Establishing  Initial  Conditions 

The  equations  from  Seetion  3.2  provide  the  linearized  EKF  model  for  aiding  an 
INS  using  the  angular  measurements  obtained  by  traeking  a  single,  stationary  landmark. 
The  extended  Kalman  filter  assumes  small  perturbations  about  a  true  trajeetory  of  the 
navigation-state  variables.  The  true  navigation  state  eannot  be  known  for  eertain,  so  a 
ealeulated  nominal  (denoted  by  the  e  subseript),  elose  to  the  truth,  must  be  ehosen.  The 
ealeulated  nominal,  provided  by  the  INS,  is  the  truth  plus  the  INS  navigation  error: 

(3.106) 

The  question  of  how  to  ehoose  the  nominal  trajeetory  is  not  trivial.  The  INS- 
derived  nav-state  provides  a  suffieient  statistie  for  the  first  9  states  of  the  EKF  nominal 
[15].  The  initial  estimate  of  the  traeked  landmark  position  requires  some  diseussion. 
Earge  errors  in  the  initial  estimate  invalidate  the  EKF  linearization.  Small  errors  are  well 
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modeled  by  the  linear  approximation,  and  provide  aeceptable  performance.  Initial 
estimate  errors  must  also  exhibit  a  zero-mean,  Gaussian  nature  in  order  for  the  EKF  to 
perform  as  expected.  The  non-linear  nature  of  optical  measurements,  and  inability  to 
deduce  three-dimensional  information  from  a  single,  two-dimensional  image,  make  for  a 
challenging  problem.  Binocular  vision  and  multiple  image  batch  estimation  techniques 
have  been  developed  and  explored  to  provide  initial  object  position  estimates  [5]. 
Hardware  and  other  such  constraints  can  make  these  approaches  impractical.  A  number 
of  monocular  initial  estimate  strategies  will  be  discussed  and  explored  in  Section  3.5.1. 
The  position  estimates  are  derived  very  differently,  but  take  the  form 

A*,=h,(>'.z)L.j  (3.107) 

where  is  a  non-linear  function  used  to  determine  the  landmark’s  relative  position. 
The  associated  uncertainties  take  the  form 

(3.108) 

As  described  in  Section  3. 1.3.2,  H^and  are  the  partial  derivatives  of  h^(x,z)  with 
respect  to  x  and  z,  and  Ppp  is  the  aircraft  position  error  co-variance.  In  this  case,  P^^  is 
defined  in  Section  3.2.2 
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Pzz=H[X,Ms(ti)]-Pxx-H[X„s(t,)f  +R 


(3.109) 


where  Pxxis  error  state  uncertainty  covariance  matrix,  R  is  the  measurement  noise 
covariance  matrix,  and  H[X|,^g(t;)]  is  the  observation  matrix  evaluated  at  the  INS-derived 
nav-state. 

3.5.1  Far  Guess  Landmark  Initial  Position  Estimation.  The  first  strategy  to 
derive  an  initial  position  estimate  involves  very  little  a  priori  information  about  the 
environment.  The  position  is  estimated  to  be  sufficiently  far  away  and  along  the  line-of- 
sight  of  the  feature  projection.  The  associated  uncertainty  ellipsoid  maintains  the  tight 
angular  uncertainty  in  the  image,  but  assigns  a  large  uncertainty  along  the  LOS  axis. 
Figure  3.13  illustrates  this  concept. 


Figure  3,13:  Far  Guess  initial  landmark  position  estimation.  The  landmark  position 
estimate  is  guessed  long  of  the  actual  target,  and  uncertainty  along  the  line  of  sight  is 
assumed  to  he  large 
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The  relative  position  estimate  is  arrived  at  by  multiplying  the  3-dimentional  unit 


vector  feature  projection  ( z  )  by  a  large  distance  {a): 


where 


^  r.  z  ^ 

aC"^^ 

"  Tz 

pa—Pliy 


T  Z 

pix—p 

T  z 

pix—p 


Zp  is  the  homogeneous  feature  pixel  location  vector,  and 
described  in  Chapter  2,  and  C”  is  the  camera-to-nav  frame  DCM. 
The  partial  derivative  of  (x,z)  is  then; 
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z 


T„(ac;:T;„) 


1  0 
0  1 
0  0 


Since  the  uncertainty  along  the  LOS  vector  is  ad  hoc,  a  is  chosen 
deviation  for  a  camera  frame  z-axis  uncertainty.  This  yields 


(3.110) 


(3.111) 


are  transforms 


(3.112) 


as  the  standard 
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HxPxxH/=T„.C:[0  0  a] 


(3.113) 


0 

0 


C„T 


L«J 

Ideally,  if  zero  knowledge  was  in  faet  the  ease,  a  would  be  equal  to  infinity,  but 
machine  precision,  and  matrix  condition  drive  a  real,  but  large  number  to  be  substituted. 

An  inherent  problem  with  this  method  is  that  it  does,  by  design,  invoke  a  large 
error  in  the  estimated  landmark  position,  complicating  the  EKF  linearization.  The  large 
error  covariance  assigned  to  the  estimate  aims  to  minimize  the  effect  of  this  bad  estimate. 
The  large  uncertainty  opens  the  search  for  a  match.  When  a  match  is  found,  the  aiding 
derived  from  the  measurement  residuals  is  directed  towards  the  dimensions  with  the 
highest  uncertainty.  This  differs  from  a  batch  estimator  in  many  ways.  A  batch  estimator 
requires  translation  in  order  to  triangulate  the  position  of  the  landmark.  Until  enough 
apparent  translation  occurs,  the  batch  estimator  can  provide  no  aiding.  By  injecting  the 
LOO  immediately  and  assigning  a  large  uncertainty  along  the  LOS,  the  EKL  can  reap  the 
benefit  of  aiding  along  the  dimensions  that  are  certain.  Lor  example,  a  landmark  on  the 
distant  horizon  exhibits  no  angular  rate  and  could  therefore  not  be  used  by  a  batch 
estimator.  The  EKL  would  gain  no  positional  aiding  from  such  a  landmark,  but  would 
gather  attitude  aiding  in  all  dimensions  accept  rotation  about  the  LOS  vector.  If  slow 
translation  is  occurring,  the  EKL  will  gradually  shrink  the  uncertainty  in  the  dimensions 
that  were  un-observable,  reaping  ever  more  reward  along  the  way.  The  batch  estimator 
waits  until  translation  has  occurred  before  using  the  landmark.  This  technique  is 
experimentally  evaluated  in  Chapter  4. 
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3.5.2  Average  Height  Landmark  Initial  Position  Estimation.  The  second 
strategy  to  derive  an  initial  position  estimate  involves  some  a  priori  information  about  the 
environment.  The  Average  Height  method  borrows  from  the  Far  Guess  method.  Instead 
of  guessing  that  the  landmark  is  far  away,  the  guess  is  made  that  it  is  roughly  co-planar 
with  the  others  being  tracked.  Figure  3.14  illustrates  this  concept. 


Direction  of  travel 


Figure  3,14:  Average  Height  landmark  position  estimation.  The  landmark  position 
estimate  intersects  the  plane  defined  hy  the  average  height  of  other  tracked 
landmarks. 


The  assumption  is  that  landmarks  close  together  have  spatially  correlated 
elevations.  The  implementation  is  identical  to  the  Far  Guess,  except  that  the  distance  a 
is  chosen  such  that  the  line  defined  by  the  unit  pointing  vector  z ,  intersects  the  x-y  plane 
defined  by  the  average  height  of  all  other  tracks.  The  strength  of  the  uncertainty  along 
the  LOS  must  be  chosen  based  on  knowledge  of  the  environment,  and  is  scaled  by  the 
inverse  sin  of  the  angle  between  the  x-y  plane  and  the  LOS  vector.  The  uncertainty  can 
still  be  large,  however.  This  is  an  engineering  judgment  call,  not  pure  science  by  any 
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means.  This  technique  does  allow  closer  estimates  over  flat  terrain,  but  is  problematic  in 
rising  or  rough  terrain. 

5.5.5  DIED  Intersection  Initial  Landmark  Position  Estimation.  The  last 
strategy  is  again  similar  to  the  Average  Height,  except  a  DTED  map  is  used  for 
determining  the  height  of  the  landmark,  not  the  others  being  tracked.  The  unit  pointing 
vector  z,  is  projected  from  the  aircraft  to  the  DTED  map.  An  altitude  and  geodetic 
position  are  resolved  recursively.  This  becomes  the  position  estimate.  Eigure  3.15 
illustrates  this  concept. 


Direction  of  travel 


Figure  3,15:  DTED  intersection  initial  landmark  position  estimation.  The 
landmark  position  estimate  intersects  DTED  map. 


The  uncertainty  in  height  becomes  a  function  of  the  resolution  of  the  DTED  map 
itself,  and  the  variance  of  terrain  elevations  in  the  immediate  area  (cTsurroundings)-  This 
variation  comes  from  searching  1  standard  deviation  around  the  intersection  point  and 
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examining  the  terrain  (aeeording  to  the  DTED  map).  Uncertainty  in  the  position 
measurement  is  now  a  sum  of  the  camera  frame  pointing  errors,  projected  on  the  Earth, 
and  the  height  uncertainty  as  described. 

The  relative  position  estimate  is  arrived  at  by  intersecting  the  DTED  map.  The 
procedure  to  do  so  is  trivial  and  not  discussed.  However,  is  no  longer  a  tractable 

function  that  can  be  differentiated,  making  the  uncertainty  derivation  more  ad  hoc.  The 
contribution  to  uncertainty  due  to  the  DTED  map  and  aircraft  navigation  state  can  be 
modeled  as  the  sum  of  DTED  uncertainty  and  the  variation  of  the  immediate 
surroundings. 


H,P„H/=[0  0  ,n,.)[»  »  1]  (3.114) 

The  remainder  is  still  due  to  camera  frame  pointing  errors.  is  still  tractable,  therefore 
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(3.115) 


where,  length  of  the  relative  position  vector  to  the  DTED  intersection 


^DTED  “  ^tgt  “  "^en'iPac  Plandmark )  (3.116) 
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where  landmark  ^he  geodetic  positions  of  the  aircraft  and  landmark. 

This  technique  gives  excellent  position  estimates  as  long  as  the  navigation  state  is 
close  to  truth.  If  the  position  or  attitude  errors  are  too  large,  the  intersection  will  become 
invalid.  This  is  by  no  means  an  independent  measurement  source. 

3.5.4  Replacing  Landmark  Tracks.  The  SLAAMR  routine  establishes  a 
navigation  state  augmented  with  a  constant  number  of  estimated  landmark  tracks.  This  is 
as  opposed  to  dynamically  building  the  state  vector  based  on  the  number  of  features 
available  in  each  image.  This  method  was  chosen  for  two  reasons.  SIFT®  consistently 
generates  hundreds  of  features  or  more,  so  it  is  impractical  to  track  all.  It  was  also  shown 
that  only  2  are  ever  needed,  but  many  should  be  tracked  to  ensure  good  aiding  action. 
Ten  to  twenty  tracks  (constant  for  a  given  scenario)  is  the  chosen  compromise.  When 
SLAAMR  initializes  or  a  landmark  falls  out  of  view,  a  new  landmark  must  be  inserted  in 
its  place.  A  new  landmark  is  chosen  and  its  position  estimated  by  one  of  the  methods 
described  above.  Its  position  estimate  (Pnew)  is  simply  added  in  the  vacant  spot  in  the 
whole  nave  state: 


\T/«  T 

^  nb 


Pnew 


T 


(3.117) 


and  the  associated  error  state  components  are  set  to  zero: 

5y.{k)  =  [5pJ  \  ^p,^/  •••  [O  0  O]  •••  (3.118) 
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The  associated  3 -dimensional  uncertainty  (P„g„)  is  inserted  into  the  error  state  covariance 


matrix  with  zeros  filling  the  cross  covariance  elements. 
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(3.119) 


as  described  in  Section  3.1.  The  zeros  filling  the  cross-covariance  elements  ensure  that 
the  new  track  is  independent  of  the  other  states  until  it  is  tracked  for  the  first  time.  This 
allows  a  seamless,  online  method  of  inserting  new  tracks  without  disrupting  the  others. 
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3,6  Principles  of  Design 

This  chapter  developed  the  theory  behind  the  design  of  the  SLAAMR  algorithm. 
A  number  of  prineiples  ultimately  drive  trade-offs  and  strategies  in  an  effort  to  maximize 
aiding  in  a  robust  environment.  They  are: 

•  Only  two  known  landmark  traeks  are  required  for  full  observability. 

•  Aiding  aetion  inereases  with  dwell  time. 

•  Surveyed  landmarks  (though  estimated  in  the  EKF)  provide  nearly  equivalent 
aiding  to  known  landmark  traeking. 

•  Through  eontinuous  traeking,  the  uneertainty  of  landmark  and  aireraft  position 
estimates  approaehes  that  of  the  lower  one. 

•  Traek  many  landmarks  simultaneously  to  dilute  unobservability. 

•  Choose  new  landmarks  to  traek,  sueh  that  the  population  evenly  spans  the  image. 

•  Inelude  landmarks  into  the  EKF  immediately  (after  a  single  image)  to  maximize 
aiding  (don’t  wait  until  triangulation  is  possible,  i.e.,  as  in  bateh  proeessing). 

•  Use  a  wide  field-of-view  lens  to  maximize  dwell  time. 

•  Point  the  eamera  down  or  baekwards  to  maximize  good  geometry  and  dwell. 

•  Do  not  point  the  eamera  forward  as  it  provides  poor  temporal  feature  flow. 

•  Passive  altitude  and  heading  measurements  improve  overall  observability. 

•  Eow  V  over  h  generally  improves  the  observability  grammian  matrix  eondition 
number  and  GDOP,  but  too  low  has  penalties. 

•  There  exist  sweet  spots  at  higher  V  over  h  that  improve  both  the  observability 
grammian  matrix  eondition  number  and  GDOP,  but  eannot  be  predieted. 

•  High  and  low  values  of  V  over  h  are  relative  and  eannot  be  generalized.  Without 
knowledge  of  these  unknown  thresholds,  the  V  over  h  span  should  be  maximized 
to  eapture  the  sweet  spots  and  eomplementary  ends  of  the  speetrum. 

•  Capturing  the  horizon  provides  attitude  observability. 

•  Capturing  nadir  improves  possibility  for  good  GDOP. 
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3. 7  SLAAMR  Algorithm  Design 

The  SLAAMR  algorithm  was  based  on  the  study  done  in  this  chapter.  It  is 
designed  to  maximize  aiding  strength  at  all  times  in  the  worst  case  navigation  scenarios. 
The  addition  of  passive  measurements,  such  as  heading  and  altitude,  complement  the 
SLAM  based  design  to  make  up  for  its  observability  lackings.  Additional  design 
elements  that  account  for  realistic  image  processing  are  implemented  as  well.  Figure 
3.16  illustrates  the  SLAAMR  algorithm  process  with  a  block  diagram. 

The  INS  and  EKF  act  as  a  conventional  feedback  system,  where  error  state  values 
are  applied  to  the  INS  nav-state  with  every  measurement.  The  same  process  applies  to 
the  landmark  position  estimates.  The  compass  and  altimeter  measurements  are  added  to 
the  system  by  simply  augmenting  the  EKF  measurement  equation. 

The  camera  is  pointed  to  optimize  feature  flow  (down  or  aft).  SIFT  algorithm 
creates  features  from  images.  These  features  are  compared  to  tracked  landmarks  to  find  a 
match.  Estimated  landmark  positions  are  used  to  narrow  the  search.  If  a  new  landmark 
track  is  desired,  a  feature  is  selected  from  the  image  based  on  its  location  in  the  frame  and 
relation  to  the  other  tracks.  Then  an  initial  position  estimate  is  made  by  one  of  the 
various  techniques  discussed,  and  the  landmark  inserted  into  the  system.  If  surveyed 
landmarks  are  available,  the  features  are  searched  for  a  match.  If  a  match  is  made,  the 
surveyed  landmark  is  inserted,  overriding  a  landmark  of  opportunity  track. 

The  estimated  measurements  (feature  projections,  heading  and  altitude)  are 
generated  by  the  navigation  state  and  landmark  position  estimates,  and  the  differences 
with  measured  values  are  fed  into  the  EKE  as  residual  errors. 
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Figure  3,16:  SLAAMR  System  Diagram, 
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3.8  Design  Summary 

This  chapter  aimed  to  explore  the  mathematieal  nature  of  bearings-only 
measurements  and  how  they  apply  to  a  SLAM-based  navigation  strategy.  The  findings 
illustrated  several  infiueneing  faetors  and  challenges  that  hinder  that  performanee  of  an 
EKF  aiding  strategy.  A  mathematieally  optimal  design  was  augmented  with  praetieal 
measures  to  overcome  realistie  problems  faeed  when  designing  a  robust  system  eapable 
of  operating  in  all  environments. 

The  importanee  of  camera  look  angle  and  a  few  critieal  additional  passive 
measurements  make  the  design  of  the  SLAAMR  algorithm  stand  apart  from  most  SLAM- 
based  systems,  and  allow  it  to  perform  well  in  realistie  environments  with  no 
artifieialities.  The  SLAAMR  design  does  not  rely  on  perfeet  and  non-existent  image 
proeessing,  orbiting  a  set  of  favorable  landmarks,  or  the  benefits  of  small  seale. 

The  next  ehapter  will  experimentally  evaluate  the  mathematieal  assertions  made 
in  this  ehapter.  Experimental  data  will  also  support  the  design  deeisions  made  by 
illustrating  their  benefit  and  the  penalties  paid  for  not  being  implemented. 


170 


IV  Flight  Test  and  Experimental  Results 


Chapter  3  established  the  mathematies  behind  the  SLAAMR  routine  and 
established  the  basic  architecture.  During  the  mathematical  development,  some 
propositions  were  made  about  the  performance  of  the  SLAAMR  algorithm  and  factors 
influencing  aiding  action.  This  chapter  will  experimentally  evaluate  both  the  SLAAMR 
algorithm,  and  underlying  principles  behind  image  aiding  of  an  INS. 

The  flight  test  equipment  and  methods  are  described,  as  well  as  limiting  factors. 
A  precision  truth  source  is  used  to  validate  the  results,  providing  clear  data  and 
conclusions.  Navigation  state  errors  and  the  statistics  of  these  errors  are  used  as 
appropriate  measures  of  performance  for  the  navigation  system. 

The  experimental  portion  of  this  thesis  was  flown  as  a  Test  Management  Project 
at  the  United  State  Air  Force  Test  Pilot  School  (USAF  TPS)  in  conjunction  with  the  Air 
Force  Institute  of  Technology  (AFIT).  The  GPS  Out-Or-Denied  Low-Cost  Kalman 
Iterated  Navigation  (GOOD  LOOKIN)  test  team  that  executed  the  flight  test  was 
comprised  of  two  test  pilots,  and  three  flight  test  engineers.  Raw  data  were  collected  to 
evaluate  the  SLAAMR  algorithm  as  a  navigation  strategy.  A  total  of  13.1  hours  on  6 
flight  test  sorties  were  flown  on  a  modified  C-12C  aircraft  in  the  R-2508  complex  and 
Palmdale/Lancaster,  CA  airspace  between  6-18  September  2007.  All  experimental 
analysis  in  this  chapter  is  derived  from  this  test. 
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4.1  Simulation 


Before  any  flight  test  was  seheduled  or  flown,  a  simulation  was  performed  to 
charaeterize  performanee.  The  SLAAMR  algorithm  was  designed  sueh  that  it  eould  be 
fed  simulated  images  and  inertial  measurements  in  the  same  manner  as  it  would  with  real 
data.  This  allowed  the  algorithm  to  go  through  many  simulated  iterations  and  ehanges 
before  flight,  with  a  high  confidenee  that  the  flight  test  algorithm  would  function  in  a 
manner  similar  to  simulation.  Performance  may  vary  with  real  data  inputs,  but  the 
functionality  of  the  algorithm  should  be  the  same. 

Each  profile  or  mode  of  data  collection  was  simulated  before  flight.  Simulated 
results  validated  the  overall  design  of  the  SLAAMR  algorithm.  The  only  difference 
between  flight  test  performance  and  simulated  performance  would  reside  in  the  nature  of 
the  measurement  data  fed  into  the  algorithm.  The  algorithm  attempts  to  model  the  nature 
and  noise  associated  with  the  inertial  and  image  measurements,  and  thus,  degraded 
performance  in  flight  test  is  likely  due  to  these  models  being  insufficient.  Simulated 
results  will  accompany  flight  test  results,  presented  later  in  this  chapter. 

4.1.1  Simulated  Images.  The  simulated  images  came  in  the  form  of  feature  sets 
(or  key  files),  identical  in  format  to  those  produced  by  the  SfFT  algorithm.  A  set  of 
features  was  generated  in  -dimensional  space,  and  assigned  geodetic  coordinates.  Each 
feature  was  assigned  a  scale,  orientation  and  128-element  descriptor  using  a  random 
number  generator.  A  true  aircraft  trajectory  was  created,  simulating  the  aircraft’s 
position,  velocity  and  attitude  in  space.  Key  fdes  were  generated  at  appropriate  time 
intervals  using  the  true  aircraft  trajectory,  and  true  landmark  positions  ). 
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The  true  pixel  eoordinates  in  eaeh  key  file  ( )  were  generated  using  the  non-linear 
measurement  equation  from  Chapter  2: 

Z.rue(0  =  h[Xt™e(t,)]  (4.1) 

The  true  pixel  eoordinates  were  generated  aeeording  to  a  pinhole  eamera  model.  Wide 
angle,  fish  eye  distortion  was  applied  to  these  coordinates  using  a  distortion  function 
(/)(•)),  derived  in  Section  4.3.2. 

Zs/M  (0  =  ^(z, rue  (h))  +  V^  (4.2) 

White  Gaussian  pixel  coordinate  measurement  noise  (v^)  was  added  according  to  the 
model  in  Chapter  2.  The  simulated  pixel  coordinates  z^^^(t)  differ  from  true 

measurements  in  that  the  noise  is  assumed  to  be  zero-mean  and  normally  distributed. 
The  test  data  are  generated  by,  and  thus  corrupted  by,  the  feature  generation  program  and 
non-linear  optics.  Additionally,  white  noise  was  added  to  the  descriptors  to  simulate 
change  frame  to  frame.  This  is  by  no  means  the  true  nature  of  feature  descriptor  change, 
which  proved  problematic,  as  will  be  shown. 
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4.1.2  Simulated  Inertial  Measurements.  The  true  trajectory  (generated  for  the 
simulated  flight  path)  was  used  to  generate  simulated  inertial  measurements.  The 


equation  derived  in  Chapter  2  is  solved  for  the  specific  force  vector  : 

f,:  =  c;  (v;  +  (2<  +  <. )  X  v;  -  g; ) + a-  +  w‘  (4.3) 

and  is  a  function  of  aircraft  nav-to-Earth  velocity  and  acceleration  (v”^,v"^)  in  the  e- 

frame,  the  accelerometer  bias  ( a*  modeled  as  a  first-order  Gauss-Markov  process),  and  a 
number  of  Earth  effects.  The  same  was  accomplished  for  the  rate  gyro  measurements 


<=<+C;(<+<)  +  wJ  (4.4) 

which  is  a  function  of  the  attitude,  body-to-nav  rotation  in  the  b-frame  ( C* ,  )  and 

Earth  effects. 

The  simulated  inertial  measurements  were  corrupted  with  white,  Gaussian  noise 
( w* ,  )  according  to  the  model  in  Chapter  2.  They  differ  very  little  from  the  flight  test 

data  because  these  kinematics  and  models  are  very  well  developed  and  understood  [23]. 
True  sensors,  however,  are  subject  to  aircraft  vibrations.  This  was  not  simulated. 
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4. 2  Test  Item  Description 

The  system  under  test  (SUT)  eonsisted  of  two  high-resolution  digital  still 
eameras,  a  miero-eleetro-meehanioal  system  (MEMS)  inertial  measurement  unit  (IMU), 
and  a  laptop  eomputer  for  data  eolleetion  and  proeessing.  The  eomponents  were  seleeted 
to  provide  a  low-eost,  preeision  navigation  system  for  use  in  GPS-denied  environments. 
The  SUT  was  earned  aboard  a  C-12C  aireraft,  allowing  an  unobstructed  view  of  the 
Earth  below.  Precision  Time  Space  Position  Information  (TSPI)  was  provided  by  the 
412*  Range  Squadron,  and  correlated  to  the  test  data. 

4.2.1  High  Resolution  Digital  Camera.  The  Pixelink  PE-A741  is  a  high- 
performance,  1.3  mega-pixel  monochrome  camera  designed  specifically  for  machine 
vision  applications.  The  camera  is  connected  to  the  computer  by  a  EireWire  interface 
[33].  It  provided  the  images  needed  for  aiding,  and  an  external  trigger  option  enabled 
GPS  time  stamping  of  the  images,  which  is  critical  for  effective  aiding  using  the 
SEAAMR  algorithm. 

The  PE-A741  had  a  selectable  resolution,  but  1024x768  was  used  for  test.  The 
shutter,  gain,  and  contrast  were  selectable  prior  to  data  runs  and  were  set  for 
environmental  conditions.  The  cameras  generated  Portable  Gray  Map  (.pgm)  images, 
which  are  free  from  compression  distortions.  The  camera  was  triggered  to  take  images  at 
a  rate  of  4  per  second.  A  higher  rate  was  desired,  but  as  it  stood,  each  image  was  784 
kilo-bytes,  and  approximately  50-60  giga-bytes  of  data  were  collected  per  hour  (two 
cameras  collecting  simultaneously). 
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The  lens  used  provided  a  field-of-view  of  63.2  degrees  vertically  and  84.7  degrees 
horizontally.  The  cameras  were  arranged  such  that  the  wider  dimension  ran  parallel  to 
flight  path,  maximizing  longitudinal  field  of  view.  The  focal  length  of  the  lens  was 
4.9268  mm,  and  the  lens  set  to  focus  on  infinity.  A  summary  of  camera  specifications 
follows.  The  focal  length  (/),  focal  plane  pixel  dimension  {MxN)  and  physical  height 
{H)  and  width  {W)  (all  used  in  Equation  (3.30))  were: 

/  =  4.9268(mm)  (4.5) 

M  =  16%,  A  =  1024  (4.6) 

H  =  5A919{mm)  W=lA044{mm)  (4.7) 

Figure  4.1  illustrates  the  compact  camera  package.  Further  specs  can  be  found  in 
reference  [33]. 


Figure  4,1:  High  resolution  digital  camera  -  PL-A471, 
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4.2.2  Inertial  Measurement  Unit.  The  Microbotics  INC.,  MIDG  II  INS/GPS 
Inertial  Navigation  System  (INS)  was  chosen  as  the  Inertial  Measurement  Unit  (IMU)  for 
this  test.  Though,  marketed  as  a  GPS/INS,  it  was  in  fact  used  simply  as  an  IMU  for  this 
test.  The  raw  inertial  measurement  data  (from  the  internal  accelerometer  and  rate  gyro 
triads)  was  collected  at  50  samples  per  second,  via  a  serial  cable.  The  internal  inertial 
sensors  were  Micro-Electro-Mechanical  Systems  (MEMS)  level  components.  The 
onboard  GPS  receiver  (connected  to  an  external  antenna)  allowed  GPS  time  stamping  of 
the  inertial  data  as  well  as  the  images.  The  GPS  data  (collected  separately  from  the 
inertial  measurements)  also  served  as  a  redundant  truth  source  in  the  event  that  the  TSPI 
failed.  The  MIDG  II  was  chosen  because  it  was  representative  of  a  MEMS  grade  sensor, 
while  providing  a  convenient  interface  and  GPS  time  stamp  capability. 

The  values  for  accelerometer  measurement  noise  (w*),  bias  process  driving  noise 
(w*^  ),  and  time  constant  (t^)  were  derived  from  ground  test  data  and  estimated  to  be 
Gaussian  with  strengths  defined  by: 

w;;  =(0.050m/5")"-5  (4.8) 

"I. 
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The  values  for  rate  gyro  measurement  noise  (w^),  bias  process  driving  noise 


( )  and  time  constant  (  )  were  estimated  to  be; 


^  =  (0.060ra(//s')^  -s 

(4.11) 

(4.12) 

(4.13) 

The  MIDG  II  was  small  (1.50”  x  1.58”  x  0.88”,  weight  under  55  grams),  and 
illustrated  in  Figure  4.2.  More  information  can  be  found  in  [34]. 


Figure  4,2:  Inertial  Measurement  Unit  -  MIDG  II INS/GPS, 
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4.2.3  Truth  Reference  Source.  A  combined  GPS/inertial  reference  system  and 
external  GPS  antennas  provided  truth  data  for  post  flight  performance  assessment.  The 
truth  reference  system  used  (GLite  Configuration  2-B,  C2B)  integrates  the  Novatel  OEM- 
V  GPS  receiver,  the  Honeywell  Ring-Laser-Gyro  HG1700AG58  IMU,  and  various  other 
Commercial  Off-The-Shelf  products  to  record  GPS/IMU  data,  produce  a  GPS/IMU 
blended  solution  in  real-time,  and  interface  with  various  monitors  and  controls  devices. 
The  data  was  recorded  via  removable  media  and  post-processed  by  the  412*’^  Range 
Squadron  at  Edwards  AEB.  The  resultant  truth  data  provided  precise  (differential  GPS 
quality)  geodetic  position,  velocity  and  attitude  data.  These  data  correspond  to  the  nine 
basic  nav-states  output  by  the  INS  under  test.  The  GLite  was  positioned  close  to  the 
MIDG  II  (2  meters  behind).  A  simple  three  axis  rotation  was  required  to  align  the  MIDG 
II  frame  of  reference  and  the  GLite.  A  precise  survey  of  the  GLite  frame  of  reference, 
aircraft  frame,  and  MIDG  II  frame  was  performed  by  Air  Lorce  Llight  Test  Center 
(ALLTC)  personnel  in  order  to  resolve  this  alignment.  Note  that  the  aircraft  frame  was  a 
relatively  arbitrary  intermediate  frame  between  the  GLite  and  MIDGII,  and  is  thus 
discarded.  The  MIDG  II  frame  will  be  considered  to  be  the  aircraft  frame  for  the  purpose 
of  this  discussion.  The  precision  and  accuracy  of  this  system  is  orders  of  magnitude 
better  than  the  system  under  test  will  achieve,  and  is  therefore  considered  a  practical 
truth. 

4.2.4  Sensor  Integration.  The  SUT  consisted  of  two  basic  sensors:  the  cameras 
and  the  IMU.  It  also  used  two  sources  of  truth  data,  the  TSPI  and  GPS  data  from  the 
MIDG  II.  All  the  data  was  collected  into  a  laptop  computer  (XPS  M1710  Special  Edition 
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Formula  Red)  carried  onboard  the  aircraft.  A  data  collection  program  (created  at  AFIT 
by  Veth  for  his  work  on  the  subject  [26])  performed  the  job  of  collecting,  sorting,  and 
controlling  the  sensors.  All  measurements  and  images  were  stamped  with  GPS  time  to 
allow  proper  time  synchronization  during  post  processing.  The  TSPI  data  and  inertial 
data  were  taken  at  a  rate  of  50  samples  per  second  and  simultaneously  on  the  1/50*  of  a 
GPS  week  second  (i.e.,  data  taken  at  0.00,  0.02,  0.04...).  Though  not  required,  this 
reduced  the  effort  required  to  correlate  the  truth  with  estimates. 

Two  cameras  were  used  at  a  time,  each  pointing  at  a  different  angle.  One  was 
declared  master  and  one  slave.  An  internal  trigger  in  the  master  camera  fired  (at  a  rate  of 
4  per  second),  commanding  the  slave  also  to  lire.  The  trigger  notified  the  data  collection 
program  that  images  were  being  taken,  and  the  GPS  time  gathered  from  the  MIDG  II  was 
them  ‘stamped’  on  the  images  in  the  form  of  the  image’s  filename.  Figure  4.3  illustrates 
a  top-level  block  diagram  of  the  SUT. 
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4.2.5  Test  Aircraft.  A  C-12C  Huron  aircraft  (shown  in  Figure  4.4)  was  chosen 
for  the  test.  The  Huron  is  a  twin  engine  turbo-prop,  utility  class  aircraft.  Its  performance 
and  operating  envelope  is  comparable  to  many  transport  aircraft  or  the  predator  UAV. 


Figure  4,4:  Test  aircraft,  C-12C  Huron, 

The  Huron  allowed  the  entire  test  team  to  be  onboard  during  test.  The  team 
comprised  of  2  test  pilots,  1  mission  director,  and  1-2  systems  operators.  Figure  4.5 
shows  the  crew  positions  and  equipment  racks. 

The  GLite  is  just  out  of  sight  in  Figure  4.5,  on  the  floor  next  to  the  orange 
equipment  rack.  The  cameras  and  IMU  are  below  the  co-pilot’s  seat  (not  in  view)  and 
external  to  the  aircraft.  There  existed  a  hatch  below  the  co-pilot’s  seat  from  a  previous 
test,  allowing  the  seamless  installation  of  the  cameras  and  IMU.  The  cameras  and  IMU 
were  rigidly  mounted  to  an  existing  apparatus  that  fits  into  this  hatch  (described  in  the 
next  section),  and  wires  run  under  the  floor  boards  to  the  computer.  The  cameras  were 
individually  configurable  to  allow  them  to  be  positioned  within  45  degrees  of  vertical. 


182 


along  the  pitch  axis.  Unnecessary  antennas  were  removed  from  the  bottom  of  the  aircraft 
to  allow  an  unobstructed  view  from  horizon  to  horizon.  Figure  4.6  shows  the  cameras  as 
installed  in  the  aircraft. 


Figure  4,5:  Crew  positions  and  test  equipment. 
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Figure  4,6:  Installed  Sensors, 

The  left  propeller  is  seen  in  the  background  of  Figure  4.6.  The  front  of  the 
aircraft  is  to  the  right  of  the  image.  In  this  figure,  one  camera  is  looking  forward,  and  one 
down,  but  they  could  be  repositioned  to  meet  the  needs  of  the  test  mission. 

The  C-12C  provided  a  stable  platform  for  flight  test.  The  turbo  props  provided  a 
relatively  low  vibration  environment.  The  test  parameters  were  well  within  the 
operational  limits  of  the  aircraft,  and  the  designed  structural  limits  of  the  camera  harness 
were  well  beyond  those  of  the  aircraft.  The  C-12C  aircraft  was  not  a  limiting  factor  for 
test,  nor  was  there  any  danger  of  damaging  the  equipment  if  in  the  C-12C  flight  envelope. 

4.2.6  Sensor  Harness.  As  mentioned  above,  the  cameras  and  IMU  were  rigidly 
mounted  to  a  sensor  harness  that  mated  to  the  pre-existing  hatch  below  the  co-pilot’s  seat. 

The  harness  slid  down  rails  and  latched,  mating  with  the  lower  fuselage,  and  forming  a 
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pressure  seal.  The  harness  was  further  modified  to  mount  the  MIDG  II IMU  and  eameras 
rigidly  and  elose  together.  The  distanee  was  small  enough  to  be  negleeted  in 
eomputation  (<l-2  inches).  Figure  4.7  show  the  harness  in  more  detail. 

The  IMU  was  roughly  aligned  with  the  natural  aircraft  body  axis,  and  thus  the 
body  frame  was  defined  to  be  equal  to  the  IMU  frame.  The  cameras  were  mounted 
coplanar  with  the  IMU,  and  set  in  pivoting  grooves  to  allow  a  +-45  degree  look  angle 
change.  Once  set  at  the  desired  look  angle,  the  cameras  could  be  securely  fastened.  One 
camera  was  aligned  with  the  IMU,  looking  straight  down,  and  locked  into  place.  This 
camera’s  frame  of  reference  and  the  IMU  frame  were  precisely  surveyed  and  never 
changed.  The  remaining  camera  was  set  to  different  look  angles  according  to  test 
requirements  (roughly  +  or  -  45  degrees),  and  its  rotation  angle  was  measured  relative  to 
the  fixed,  downward  camera.  Since  the  cameras  were  coplanar,  only  the  pivot  angle 
needed  to  be  measured. 

The  harness  was  designed  to  withstand  the  dynamic  air  pressure  created  by  flying 
at  250  knots  indicated  airspeed  with  a  margin  of  safety  of  1.6.  It  was  flight  tested  to  220 
knots  without  deformation  or  vibration. 
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Figure  4,7:  Sensor  harness. 
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4.3  Test  Item  Calibration  and  Validation 


The  underlying  theory  behind  aiding  a  low-eost  INS  with  bearings  measurements, 
assumes  that  certain  parameters  are  known  or  well  modeled.  Bearings  measurements 
from  cameras  require  high  image  photo  resolution  for  precision  and  a  true  angular  survey 
for  accuracy  (called  a  boresight).  The  image  resolution  not  only  applies  to  the  resolution 
of  the  image  itself,  but  accounts  for  the  feature  generation  process,  and  in  part,  the 
pinhole  camera  model  calibration  used.  The  angular  survey  requires  precise  knowledge 
of  camera  alignment  to  the  IMU  frame  (boresight),  field  of  view  and  pinhole  camera 
model  calibration. 

The  Kalman  filter  also  assumes  certain  properties  of  the  feature  generation 
system.  The  EKF  model  performs  best  when  features  are  tracked  until  no  longer  in  view 
(to  maximize  dwell  time).  The  ability  of  the  feature  generator  to  place  a  feature 
consistently  on  a  landmark  drives  performance  and  process  efficiency.  The  nature  of  the 
feature  generator  must  be  well  understood  and  the  algorithm  optimized  for  accuracy, 
precision,  and  persistence. 

This  section  discusses  the  process  used  and  results  from  the  calibration  of  the  test 
equipment  and  feature  generation  software.  Ground-based  techniques  as  well  as  precise 
truth  data  from  flight  test  are  used  to  derive  and  validate  the  test  equipment  calibration 
and  characterization. 

4.3.1  Camera  Field-of-View.  The  field-of-view  of  the  camera  is  required  to 
deduce  the  effective  size  of  the  imaging  array.  This  measurement  is  required  for  the 
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pinhole  camera  model.  Width  (JV)  and  Height  (B)  are  defined  by  function  of  the 


horizontal  and  vertical  fields  of  view  {FOV^ ,  FOV^)  . 


W  =  2-  f  ■  tan 


FOV. 


FI  =  2-  f  ■  tan 


(4.14) 


The  ultimate  goal  of  calibration  was  to  reduce  errors  to  less  than  1  pixel.  A  point 
source  object  (comer  of  a  checkerboard  panel)  was  placed  far  from  the  camera  and  such 
that  it  was  projected  near  the  edge  of  the  camera  field  of  view.  The  distance  and  angles 
from  the  camera  focal  point  and  the  object  were  precisely  measured,  and  field  of  view 
determined  with  trivial  geometry.  This  was  repeated  on  both  sides,  and  in  both  axes  to 
find  asymmetry.  The  center  of  the  field  of  view  must  also  be  known,  and  expressed  as  a 
pixel  location  to  define  the  z-axis  of  the  c-frame.  Since  physical  surveys  were  made 
relative  to  the  physical  camera  shell,  so  must  the  camera  frame  projection  in  the  image. 
Lasers  were  aligned  to  the  shell  on  both  sides,  and  swept.  The  intersection  of  the  laser 
traces  on  a  far  away  board  (as  seen  in  the  image),  defined  the  center  pixel.  This  was 
compared  with  the  results  of  the  pinhole  camera  calibration  process  defined  in  [36]. 

4.3.2  Pinhole  Camera  Model  Calibration  and  Results.  The  first  objective  of 
this  test  was  to  determine  pinhole  camera  calibration  error  model  for  the  wide  angle 
digital  cameras.  The  lens  of  the  camera  distorted  the  image  in  a  non-linear  fashion.  The 
wide  field  of  view  (FOV)  caused  a  “fish  eye”  effect,  producing  an  image  that  compresses 
features  near  the  edge.  This  distortion  was  inconsistent  with  the  pinhole  camera 
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approximation  used  in  the  measurement  equation.  The  fish-eye  effeet  had  to  be  removed 
to  approximate  a  pinhole  eamera.  The  distortion  was  modeled  as  a  sixth-order 
polynomial  funetion  of  radial  distance  (from  the  center  of  the  image).  An  un-distort 
transform  and  its  inverse  (the  distort  transform  T)(*))  were  created  to  convert 

image  coordinates  between  real,  measured  and  distorted  coordinates  ( )  and  pinhole 
camera  image  space  containing  undistorted,  calculated  coordinates  (z„). 

i=\ 

D{.)=u(.y' 

The  function  £>(•)  relies  on  iteration,  thus  a  small  error  can  be  induced  by  repeating  the 

distort/un-distort  process  back  and  forth  (z^  For  the  purpose  of  this  evaluation, 

the  error  is  negligible  for  single  transformations. 

A  discussion  of  the  process  to  derive  this  model  and  transform  is  contained  in 
[36].  It  is  a  well  understood  process,  and  not  original  work  in  this  thesis.  Thus  this 
process  will  not  be  discussed  in  detail.  However,  Figure  4.8  shows  the  result  of  the 
distortion  removal  calibration. 
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Figure  4,8:  The  effects  of  the  pinhole  camera  calihration  and  transform. 


To  speed  up  the  SLAAMR  algorithm,  the  pinhole  camera  calibration  model  was 
not  applied  to  the  actual  images,  but  simply  applied  to  all  feature  image  locations 
generated  by  the  SIFT®  algorithm.  The  resultant  feature  locations  were  then  expressed  in 
the  focal  plane  of  a  pinhole  camera.  This  method  also  prevents  feature  distortion  and 
aliasing  that  may  be  a  result  of  transforming  the  whole  image,  and  then  running  SIFT®. 

In  order  to  determine  the  camera  calibration  error  model,  truth  camera  position, 
attitude  data  (derived  from  TSPI),  and  surveyed  landmark  positions  were  used  to 
correlate  predicted  and  measured  landmark  image  projections.  The  difference  between 
estimated  and  measured  image  projection  was  considered  error.  Errors  were  measured 
across  the  image  plane  and  a  calibration  error  model  was  generated.  The  errors  were 
characterized  by  mean  and  standard  deviation,  as  well  as  spatial  dependence  across  the 
image. 
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Precise  surveyed  landmark  data  were  obtained  by  over-flying  precisely  surveyed 
landmarks  on  the  Edwards  Air  Force  Base  Photo  Resolution  Road  (PRR).  The  PRR  is 
populated  with  targets  of  varying  sizes  and  shapes  that  are  correlated  to  precise  geodetic 
coordinates.  The  non-linear  measurement  equation,  evaluated  for  the  truth  data 

z«=h[XTSPi]  (4.15) 

generates  the  predicted  image  projection  in  the  pinhole  camera  image  space. 

The  actual  landmark  was  manually  tracked  frame-to-frame,  generating 
measurements  in  distorted,  real  image  space  ( ).  The  predicted  image  projections  were 

transformed  into  real  image  space  and  differenced  with  the  predictions.  The  result  was  a 
set  of  error  vectors  associated  with  portions  of  the  image. 


=  Z)(h[Xj5p|])  — 


(4.16) 


Figure  4.9  illustrates  the  process  of  generating  error  vectors  through  a  sequence  of 
images. 
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Figure  4,9:  Tracking  surveyed  landmark  to  generate  calibration  error. 


Several  landmarks  in  the  Simulated  Combat  Airfield  and  Taetieal  Target 
(SCATT)  Range  of  the  PRR  were  used.  Sets  of  error  veetors  were  formed,  spanning  all 
four  quadrants  of  the  eamera  frame  and  radial  distanees.  Beeause  traeking  is 
aecomplished  manually,  these  errors  are  independent  of  the  feature  generation  program. 
The  resultant  error  veetors  were  fit  to  a  veetor  field  to  produee  a  model  of  the  errors 
indueed  by  the  optics,  alignment  (discussed  later),  and  the  pinhole  camera  model 
calibration  (the  latter  being  the  largest  contributor  after  calibration  is  complete).  Figure 
4.9  contains  the  results  from  the  calibration  of  one  of  the  PL-A741  cameras.  Both 
cameras  demonstrated  statistically  similar  errors,  and  were  modeled  the  same. 

For  use  with  a  Kalman  filter,  the  errors  should  be  well-modeled  by  independent, 
Gaussian  random  variables,  or  have  a  small  magnitude.  The  error  vectors  are  definitely 
not  independent  or  normally  distributed.  However,  the  errors  are  small,  with  a  maximum 
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of  2.0  pixels  and  standard  deviation  of  1.1  pixels.  Figure  4.11  illustrates  the  absolute 
magnitude  of  the  errors  (from  the  same  eamera).  Due  to  the  low  magnitudes  and 
quantization  effects  of  digital  images,  it  was  determined  that,  for  the  purposes  of  the 


Kalman  filter,  the  errors  could  be  well  modeled  by  a  zero-mean  Gaussian  random 
variable  with  a  standard  deviation  of  approximately  2  pixels.  Considering  the 
computational  cost  and  many  other,  much  more  significant  sources  of  error  (such  as 


feature  generation),  storing  this  error  map  to  subtract  these  errors  was  not  judged 
worthwhile. 


Figure  4,10:  Errors  induced  by  optics  and  pinhole  camera  calibration. 


The  dots  in  Figure  4.10  represent  the  base  of  the  error  vectors.  The  small  square 


(bottom  left)  represents  1x1  pixel  error  magnitude.  Note  this  is  a  different  scale  than  is 
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used  for  pixel  location.  The  error  vector  magnitudes  are  exaggerated  for  viewing.  The  X 
and  Y  axis  represent  pixel  location  in  the  image. 


Figure  4,11:  Errors  induced  by  optics  and  pinhole  camera  calibration  (magnitude). 


4.3.3  Sensor  Boresight.  The  term  boresight  refers  to  measuring  the  angles 
between  the  camera,  IMU,  and  TSPI  frames.  These  angles  are  then  used  to  create  the 
DCMs  required  to  transform  vectors  (between  frames). 

The  resolution  of  the  cameras  called  for  a  boresight  accurate  to  0.1  degree  in 
order  for  errors  to  be  on  the  order  of  1  pixel.  The  boresight  was  accomplished  before 
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flight  using  precision  survey  equipment  and  personnel  furnished  by  the  AFFTC,  meeting 
this  requirement.  The  boresight  was  validated  through  flight  test  by  over-flying  surveyed 
landmarks,  as  described  in  the  previous  section.  The  boresight  was  used  in  the 
determination  of  the  pinhole  camera  calibration  error  model.  Errors  in  the  boresight  may 
have  contributed  to  the  error  attributed  to  the  pinhole  calibration,  but  since  the 
magnitudes  were  so  small  (1  pixel  standard  deviation)  the  boresight  was  considered  very 
accurate. 

The  same  process  was  repeated  for  the  camera  looking  forward/aft.  Iteration  was 
used  to  resolve  the  camera  pivot  angle  (look  angle),  as  it  was  the  only  angle  not  surveyed. 
Again,  cumulative  errors  were  on  the  order  of  1  pixel  (standard  deviation),  and  the  look 
angle  boresight  was  considered  very  accurate.  By  virtue  of  small  cumulative  errors 
expressed  in  the  image  projections,  all  boresight  angles  and  values  provided  by  the 
ground  test  were  considered  very  accurate. 

4.3.4  Feature  Generation.  The  SIFT®  transform  was  used  as  the  primary 
feature  generation,  and  ultimately  matching,  algorithm.  It  has  proven  quite  robust  in 
similar  work  by  Veth  [26].  The  environment  experienced  during  the  test  proved  to  be 
challenging  however.  All  flight  test  data  were  collected  while  flying  over  California  high 
desert  and  urban  settings.  The  combination  of  the  scenery’s  makeup,  lighting,  and 
contrast  led  to  an  order  of  magnitude  more  features  per  image  than  Veth  encountered 
when  testing  over  Ohio  towns  and  farmland  (4,000-9,000  features  per  image).  This 
number  of  features  was  too  much  to  process,  so  they  were  fdtered  by  scale  (a  measure  of 
size  provided  by  SIFT®).  The  lowest  scale  features  were  thrown  out  to  yield  400-600 
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features  per  image.  SIFT®  proved  very  eapable  of  produeing  ample  features  in  even  the 
most  sparse  sections  of  the  Mojave  Desert.  A  lack  of  features  was  never  a  limiting  factor 
on  performance.  This  wasn’t  the  case  over  water  or  dry  lakebeds,  where  contrast  was  so 
low,  that  no  features  were  produced. 

4.3.5  Feature  Persistence.  The  SLAAMR  algorithm  provides  a  navigation 
solution,  but  requires  landmarks  to  be  tracked  within  a  sequence  of  consecutive  images. 
The  SLAAMR  algorithm  itself  does  not  do  any  image  processing,  but  relies  on  the  SIFT 
algorithm  do  feature  generation  and  ultimately,  matching.  As  discussed  in  Chapter  3,  a 
feature  exists  in  an  image  and  corresponds  to  a  ground  stabilized  point.  The  SIFT® 
algorithm  generates  features  in  images,  each  with  a  unique  descriptor  that  correlates 
strongly  to  the  same  feature  in  a  successive  image. 

Initial  analysis  of  frame-to-frame  feature  correlation  (for  the  flight  test  data) 
showed  poor  feature  persistence  between  frames.  Figure  4.12  shows  two  sequential 
frames  from  a  test  flight  over  urban  terrain. 

The  lightly  colored  dots  represent  features  generated  by  SIFT®.  The  crosses 
represent  features  selected  for  matching  (reduced  set  for  readability),  and  the  lines 
between  the  crosses  represent  the  best  match  in  the  subsequent  frame,  according  to 
Equation  (3.13).  In  the  ideal  case,  the  lines  would  all  be  parallel  and  of  near  equal  length. 
As  can  be  seen  in  the  figure,  there  are  some  false  positive  matches,  and  these  are 
associated  with  low  correlation  coefficients.  Through  careful  feature-by-feature 
examination,  it  was  determined  that  the  vast  majority  of  false  positive  matches  had 
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correlation  coefficients  less  than  0.9.  This  is  not  an  absolute  rule  by  any  means,  but 
provides  a  sound  decision  model. 


Figure  4,12:  Sequential,  side-by-side  urban  images  marked  with  features  and 

correlated  matcbes. 

Figure  4.13  illustrates  the  consecutive  frame-to-frame  feature  correlation 
coefficients  for  15-second  samples  of  images,  plotted  as  histograms  over  time.  The 
correlation  coefficients  for  all  features  identified  in  the  image  are  plotted  in  20  adjacent 
bins  on  the  horizontal  axes  against  the  frame  number.  The  correlation  coefficients 
represent  the  values  of  the  'best’  matches  in  the  subsequent  frame.  The  vertical  axis 
indicates  the  frequency  of  occurrence  of  an  individual  correlation  bin.  The  frequency 
axis  is  the  normalized  fraction  of  all  feature  correlations  that  fell  into  the  respective  bin. 
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The  frame  number  axis  is  meant  to  show  trends  as  a  function  of  time  (frame-to-frame 
behavior). 


0.65  0 

Correlation  Coefficient  „^^ber 

Figure  4,13:  Frame-to-frame  Correlation  Coefficients, 


A  correlation  value  of  0.90  or  greater  appears  to  be  a  good  choice  for  the 
“Positive  Match”  threshold.  Repeating  the  process  shown  in  Figure  4.1 1,  and  eliminating 
correlations  below  0.9,  supports  the  choice  of  0.9  as  the  threshold.  This  choice 
minimizes  (experimentally)  both  alpha  and  beta  errors  (false  negative  and  positive 
matches).  The  SLAAMR  algorithm  does  not  simply  search  for  the  best  match  in  the  next 
frame.  It  employs  a  spatially  constrained  search  using  the  stochastic  projection  of  the 


feature’s  estimated  position  (as  described  in  Section  3.2.2).  The  resultant  search  volume 

198 


yields  a  small  subset  of  features  for  rnateh  eonsideration  immediately  surrounding  the 
estimated  projeetion.  If  a  true  mateh  is  available,  it  will  be  ineluded  in  this  subset  of 
features.  The  nature  of  the  eorrelations  was  sueh  that  true  positive  matehes  have 
eoeffieients  ranging  from  0. 9-1.0  and  false  positive  matches  ranged  between  0.7-0. 9.  The 
spatial  search  constraint  did  nothing  to  increase  true  positive  matches,  but  minimized 
false  positive  matches  when  the  next  best  match  had  a  coefficient  near  the  threshold  of 
0.9  {next  best  matches  statistically  did  not  fall  inside  the  search  volume). 

The  problem  introduced  by  these  test  results  is  that,  as  a  best  case,  only  80-90 
percent  of  the  features  have  matches  in  the  subsequent  frame.  This  suggests  a  possible 
10-20  percent  loss  in  features  tracks  for  every  frame.  Some  of  the  lost  matches  are  due  to 
a  small  number  of  features  falling  out  of  the  field  of  view,  but  as  seen  in  Figure  4.12,  this 
is  not  the  driving  factor  (this  is  a  slow  speed,  therefore,  small  translation  condition).  If 
this  loss  is  random  and  cumulative,  the  tracker  would  expect  to  drop  all  tracks  in  short 
order.  The  behavior  over  many  frames  must  be  evaluated. 

Figure  4.14  compares  histograms  of  the  correlation  coefficients  with  increasing 
frame  spacing.  To  provide  navigation  aiding,  a  feature  must  be  tracked  consistently  over 
many  frames  of  video.  The  histograms  show  that  the  number  of  positive  matches 
(correlation  coefficient  >0.90)  decreases  and  the  number  of  rejected  matches  or  lost 
tracks  (correlation  coefficient  <0.90)  increases  as  the  frame  spacing  increases. 
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Figure  4,14:  Correlation  coefficients  with  increasing  frame  interval  spacing. 


As  frame  spacing  increases,  the  mass  shifts  from  right  of  the  decision  threshold, 
to  left.  The  effective  loss  is  not  cumulative,  but  this  does  illustrate  that  feature 
persistence  is  a  problem.  By  the  tenth  frame,  most  of  the  features  do  not  have  a  match, 
by  the  stated  criteria.  In  either  case,  either  the  feature  generation  software  and/or  the 
technique  used  to  match  features  demonstrate  poor  performance  with  regard  to  frame-to- 
firame  persistence.  Two  mitigating  techniques  are  used  to  isolate  the  SLAAMR 
algorithm’s  performance  from  the  questionable  feature  generation  (and/or  matching) 
persistence. 


200 


43.5.1  Descriptor  Handojf.  The  single  frame  interval  persistenee,  though  not 
perfeet,  greatly  outperforms  the  2  and  3  frame  intervals.  For  this  reason  it  is  benefieial  to 
exploit  the  single  frame  persistenee  with  every  mateh.  This  is  done  by  ‘handing  off  the 
feature  deseriptor  every  time  a  match  is  made.  The  algorithm  does  not  compare  features 
to  the  original  descriptor  found  when  the  track  started,  but  to  the  descriptor  from  the  most 
recent  frame.  The  descriptor  to  match  is  handed  off  to  system  memory  along  with  the 
measured  projection  and  estimated  position.  This  technique  could  lead  to  descriptor 
wander  (and  therefore  track  wander),  but  the  spatially  constrained  search  volume 
(provided  by  the  predicted  landmark  projection)  minimizes  this  effect. 

4. 3. 5.2  Non-Causal  Look-Ahead.  The  frame -to-frame  persistence  problem  is 
not  driven  by  a  random  frame-to-frame  attrition.  Thus  it  can  be  asserted  that  some 
features  naturally  persist  better  than  others.  No  online  method  was  determined  (despite 
plenty  of  efforts)  to  determine  which  features  fell  into  this  favorable  category.  The  non- 
causal  look-ahead  method  involves  taking  a  feature  in  one  frame,  and  searching  future 
frames  to  ensure  that  it  does,  in  fact,  persist.  In  the  absence  of  a  time  machine,  this 
technique  can  be  accomplished  off-line  (post-processing)  or  with  a  fixed  interval 
smoother  [14] [15] [16].  Essentially,  this  involves  running  the  aided  algorithm  at  a  fixed 
interval  (that  required  for  look-ahead),  and  inertially  propagating  (from  the  most  recent 
aided  solution)  into  real  time.  Experimentally,  looking  ahead  2  frames  and  applying  the 
same  match  decision  threshold  of  0.9  (correlation  coefficient  value),  proved  to  mitigate 
most  of  the  persistence  problems  encountered.  Two  frames  at  4  frames  a  second,  yields  a 
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V2  second  delay  due  to  the  fixed  interval  smoother;  an  acceptable  interval  for  a  real-time 
navigation  routine  in  this  application. 

A  fixed  interval  smoother  is  not  used  in  the  experimental  results  herein.  The 
look-ahead  is  merely  used  as  a  technique  to  isolate  the  SLAAMR  algorithm  from  poorly 
performing  image  processing  techniques,  which  are  not  the  focus  of  this  study. 

4.4  Flight  Plan 

The  flying  portion  of  the  test  had  one  goal:  to  collect  data.  The  data  collected  was 
then  used  to  evaluate  the  SLAAMR  algorithm  (and  the  factors  influencing  performance) 
experimentally.  The  flight  plan  was  simple:  fly  a  constant  velocity  and  altitude  over  a 
desired  area.  The  majority  of  the  data  evaluated  herein  was  collected  by  over- flying  the 
cities  of  Palmdale  and  Lancaster,  CA,  at  3,000-4,000  ft  above  ground  level  (AGL).  The 
airspeed  was  modulated  to  achieve  190  knots  of  ground  speed,  and  monitored  with  a 
portable  GPS  unit.  This  nominally  gave  a  V  over  h  ratio  of  0.08-0.12  (sec"').  The  terrain 
was  generally  fiat  and  populated  with  urban  features.  Figure  4.14  illustrates  the  general 
flight  path,  which  lasted  approximately  40-50  minutes. 

For  every  flight,  two  cameras  were  carried,  set  to  two  different  angles  (1  directly 
down,  and  1  either  45  degrees  forward  or  aft).  This  allowed  a  side-by-side  comparison  of 
the  effects  of  camera  look  angle  on  aiding  strength.  In  order  to  evaluate  the  effects  of 
tracking  differing  numbers  of  landmarks,  either  surveyed  or  landmarks  of  opportunity, 
the  data  from  each  flight  was  merely  processed  differently.  No  additional  flights  were 
required.  In  order  to  evaluate  the  effects  of  V  over  h,  different  altitudes  were  flown.  The 
airspace  did  not  allow  this  to  be  accomplished  over  the  city,  and  was  therefore 
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accomplished  over  the  uninhabited  desert.  This  did  not  allow  a  simple,  single  variable 
study  of  V  over  h  (urban  features  and  desert  features  are  different  in  nature),  but  a  general 
trend  was  evident.  The  flight  test  data  were  partitioned  into  10  independent  5 -minute 
segments.  Theses  10  segments  were  used  to  build  Monte  Carlo  plots  and  capture  the 
statistical  nature  of  the  navigation  errors. 


Figure  4,15:  Flight  path  flown  over  Palmdale  and  Lancaster,  CA, 


The  collective,  2  sigma  error  (2  standard  deviations)  for  each  measure  are 
plotted  against  time  in  the  following  sections.  In  other  cases,  30-minute  continuous  data 
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runs  are  examined.  This  provides  a  long  term  evaluation  of  the  algorithm’s  performanee, 
but  is  a  single  instanee,  and  no  statistieal  signifieanee  ean  be  gathered. 

Other  profiles  were  flown  to  eollect  data  during  operationally  representative 
maneuvers.  These  profiles  do  not  eontribute  as  mueh  to  the  pure  characterization  of  the 
algorithm,  but  speak  to  the  operational  viability  of  the  system.  These  profiles  include 
flying  lower  over  the  desert  to  achieve  a  higher  V over  h,  and  straight-in  landings. 

4.5  Results 

This  section  presents  and  evaluates  the  results  gathered  from  flight  test.  When 
appropriate,  simulated  results  will  accompany  flight  test  results,  and  comparisons  will  be 
made.  Flight  test  results  will  address  the  propositions  and  design  decisions  made  in 
Chapters.  Test  data  will  be  used  to: 

•  Validate  the  assertion  that  inserting  surveyed  landmarks  into  an  extended  Kalman 
filter  (that  augments  the  navigation  state  vector  to  estimate  landmark  position)  is 
the  practical  equivalent  to  assuming  position  is  known. 

•  Prove  that  only  two  tracks  (of  known  or  surveyed  landmarks)  are  required  for  full 
observability  and  strong  aiding  action. 

•  Validate  the  assertion  that  the  EKF  will  naturally  self-survey  landmarks. 

•  Evaluate  zero  a  priori  knowledge  landmark  tracking  for  navigation. 

•  Demonstrate  the  benefit  and  necessity  of  augmenting  image-derived  bearings 
measurements  with  additional  information  (altitude  and  heading  as  well  as  digital 
terrain  data  to  estimate  initial  landmark  position). 
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•  Demonstrate  the  benefit  of  traeking  many  landmarks  when  positions  are 
unknown. 

•  Demonstrate  the  effeet  of  look  angle  on  aiding  strength,  and  validate  the  assertion 
that  looking  forward  provides  the  poorest  performanee  (down  or  baekwards  is 
optimal). 

•  Demonstrate  the  effeet  of  V  over  h  on  aiding  aetion. 

This  ehapter  will  also  demonstrate  the  fluid  nature  of  the  SLAAMR  algorithmie 
design,  allowing  seamless  transition  between  traeking  modes  (LOO  and  surveyed  traeks). 
This  will  be  shown  through  demonstrating  navigation  with  a  partial  set  of  surveyed 
landmarks.  The  results  from  an  operationally  representative  landing  profile  will  be 
diseussed.  Lastly,  the  viability  of  a  passive,  optieal-inertial  navigation  system  will  be 
demonstrated  with  a  long  duration  flight  that  performs  eomparably  to  a  eonventional 
navigation  grade  INS. 

The  primary  measure  of  performanee  eonsidered  is  the  overall  drift  of  the 
navigation  solution.  The  drift  will  be  quantified  by  examining  the  eireular  and  spherieal 
position  and  veloeity  errors  over  time,  as  well  as  individual  attitude  state  errors.  There 
are  many  measures  to  evaluate  these  errors,  but  the  mean  and  standard  deviations  will 
emphasized  herein.  Speeifieally,  the  absolute  mean  summed  with  two  times  the  standard 
deviation  (mean  plus  2  sigma)  is  used  as  an  approximate  95%  eonfidenee  bound  for  the 
errors.  The  errors,  speoifieally  the  eireular/spherieal  position  and  veloeity  error,  were 
non-zero  mean  and  plagued  with  many  systematie  eharaeteristies  (ereated  by  image 
proeessing  and  traeking  performanee).  They  did  not  eonform  to  any  eonventional 
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random  variable.  The  mean  plus  2  sigma  measure  provides  a  eonsistent  eonfidenee 
interval  for  the  data  eolleeted,  where  other  measures  (sueh  as  root-mean-square)  did  not 
perform  as  well  (in  this  experiment).  When  applieable,  single  influeneing  faetors  will  be 
varied  to  show  the  impaet  on  performanee. 

The  overall  goal  of  this  report  is  to  eharaeterize  the  true  aiding  potential  that 
traeking  landmarks  ean  give  to  an  airborne  INS.  Additionally,  it  aims  to  expose  the 
ehallenges  of  sueh  a  problem. 

4.5.1  Effect  of  Surveyed  Landmarks.  The  assertion  was  made  that  traeking  well- 
surveyed  landmarks  yields  performanee  near  that  of  traeking  perfeetly  known  landmarks. 
This  will  be  shown  to  be  true  despite  not  having  a  full-rank  observability  grammian 
matrix.  In  the  perfeetly  known  ease,  the  attitude  angular  errors  are  on  the  order  of  the 
eamera/feature  generator  angular  resolution.  The  position  and  veloeity  errors  are 
bounded  and  small.  Figures  4.16  through  4.18  display  the  results  of  three  sets  of  Monte 
Carlo  runs,  where  2,  3,  and  10  surveyed  landmarks  are  traeked  at  any  given  time.  The 
three  runs  span  eaeh  look  angle  (forward,  downward  and  rearward). 

Eaeh  Monte  Carlo  run  eonsisted  of  8-10,  5-minute  data  segments.  The  aireraft 
was  flown  at  approximately  1000-1300  meters,  at  approximately  100  m/s,  over  flat  urban 
terrain  as  deseribed  in  Seetion  4.4.  The  mean  plus  2  sigma  (2  standard  deviations) 
spherieal  position,  veloeity,  and  attitude  errors  are  displayed  for  eaeh  eondition. 
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Figure  4,16:  Tracking  surveyed  landmarks  with  REARWARD  looking  camera. 
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Figure  4,17:  Tracking  surveyed  landmarks  with  DOWNWARD  looking  camera. 
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Yaw  Pitch  Roll  Spherical  Velocity  Error  Spherical  Position  Error 
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Airspeed:  100  m/s 
Camera  Angle:  Forward 
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Figure  4,18:  Tracking  surveyed  landmarks  with  FORWARD  looking  camera. 
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In  all  cases,  the  errors  were  bounded  and  relatively  small,  but  there  was  a  distinct 
performance  differenee.  The  rearward  looking  eamera  outperformed  the  downward  and 
forward  in  all  oases  by  a  signifioant  margin.  The  rearward  looking  eamera  errors  are 
approximately  half  of  those  for  the  downward  and  forward  oameras  by  all  measures.  As 
desoribed  in  Chapter  3,  the  rearward  eamera  provided  more  favorable  feature  flow, 
guaranteeing  better  GDOP  and  oondition  number.  When  3  or  more  surveyed  landmarks 
are  traoked,  performanoe  is  greatly  improved.  All  oonditions  showed  stable  errors  (with 
respect  to  time).  The  errors  fluctuated,  but  did  not  tend  to  drift  and  grow.  Traoking 
surveyed  landmarks  provide  a  stable  navigation  solution,  implying  virtually  full 
observability,  matehing  drift  behavior  predietions  for  traoking  known  landmarks. 

The  rearward  looking  eamera  oondition  performed  the  best  overall.  As  oan  be 
seen  in  Figures  4.16  through  4.18,  there  is  a  slight  performanoe  advantage  to  tracking  3 
versus  2  surveyed  landmarks.  There  is  only  a  marginal  advantage  to  traoking  10  versus 
3.  The  downward  and  forward  eamera  angle  oonditions  showed  worse  performanoe  than 
the  aft,  but  that  is  due  to  greater  GDOP.  Tracking  2-3  surveyed  landmarks  also 
demonstrated  poorer  overall  performanoe  than  traoking  10.  Due  to  so  few  landmark 
traoks,  the  GDOP  fluotuated  as  the  traoking  geometry  ohanged.  This  oaused  momentary 
error  exoursions,  whioh  reoovered  as  the  GDOP  later  decreased.  Traoking  10  landmarks 
provided  better  average  GDOP  over  time,  and  thus  fewer  exoursions.  These  GDOP 
artifaots  would  be  present  if  traoking  perfeotly  known  landmarks  (assuming  a  real  eamera 
is  used). 
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As  predicted  in  the  perfectly  known  landmark  position  case,  flight  test  attitude 
errors  are  generally  on  the  order  of  the  optical/feature  generation  resolution.  The 
collective  optic  and  feature  generation  noise  (corrupting  bearings  measurements)  was 
modeled  to  be  2  pixels  standard  deviation,  which  is  equivalent  to  approximately  0.2 
degrees.  When  tracking  10  landmarks  looking  aft,  the  2  sigma  attitude  errors  are  on 
average  0.4-0. 5  degrees  (Figure  4.16),  making  the  standard  deviation  0.2-0.25  degrees. 
Tracking  fewer  landmarks  caused  excursions  away  from  this  lower  limit  of  0.2  degrees, 
but  generally  returned.  Attitude  error  magnitude  depends  on  the  dimension  examined. 
The  heading  errors  are  on  the  order  of  the  camera  resolution  (0.2  degrees  standard 
deviation  or  sigma).  Pitch  errors  are  complicated  by  the  fact  that  the  aircraft  is  traveling 
forward;  feature  translation  due  to  forward  velocity  is  ambiguous  with  pitch  rate.  For  the 
test  conditions,  the  feature  angular  translation  at  nadir  due  to  forward  velocity  (frame-to- 
firame)  is  approximately  1.5  degrees.  For  the  poorly  performing  downward  looking 
camera  case,  1.5  degrees  is  approximately  the  magnitude  of  the  pitch  standard  error.  The 
forward  and  rearward  looking  cameras  benefit  from  tracking  landmarks  near  the  horizon. 
On  the  horizon,  all  feature  translation  is  due  to  rotation,  and  helps  to  resolve  the  pitch  rate 
and  velocity  ambiguities.  Roll  errors  are  naturally  higher,  since  none  of  the  conditions 
capture  a  lateral  horizon  and  the  tracked  landmark  geometry  trended  along  the 
longitudinal  axis.  GDOP  is  naturally  poorer  about  the  roll  axis.  These  GDOP  effects 
would  be  present  in  the  perfectly  known  landmark  case,  thus  the  flight  test  data  agrees 
with  prediction.  More  precise  optics  and  feature  generation  would  naturally  yield  better 
attitude  performance. 
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The  magnitude  of  the  navigation  error  is  a  function  of  GDOP  and  optics.  For  the 
best  GDOP  condition  (the  rearward  looking  case  tracking  10  surveyed  landmarks),  the 
standard  error  (1  sigma)  was  approximately  12-13  meters.  It  was  shown  that  an  aircraft 
positional  uncertainty  (when  using  perfect  optics)  will  approach  that  of  the  tracked 
landmark.  If  the  landmark  position  is  perfectly  known,  the  lower  limit  is  zero.  Since  the 
camera  used  was  not  perfect,  the  lower  limit  is  driven  by  the  optical  resolution,  as 
projected  on  the  landmark.  For  this  flight  test  example,  the  0.2  degree  standard  error 
camera,  projects  a  3. 5-4. 5  meter  standard  error  on  the  ground,  1000-1200  meters  away. 
The  camera  resolution  error  is  on  the  order  of  the  position  errors,  but  there  is  more  to 
consider.  The  attitude  uncertainty  also  contributes  to  the  lower  limit  bound  for  perfectly 
known  landmark  tracks.  In  the  known  landmark  position  case,  the  attitude  errors  are 
limited  by  the  camera  angular  resolution.  In  the  best  case,  this  adds  another  0.2  degree, 
and  3. 5-4. 5  meters  of  standard  error  on  the  ground.  Considering  this  case,  the  flight  test 
data  agrees  reasonably  well  with  the  perfectly  known  case.  Notably,  flying  higher  with 
the  same  camera  will  result  in  larger  position  errors,  and  vice  versa. 

Velocity  errors  require  a  more  complex  examination.  The  velocity  states  are  not 
included  in  the  measurement  equation.  Velocity  state  aiding  comes  via  their  correlation 
with  position  errors;  position  is  the  integral  of  velocity.  For  velocity  to  be  aided,  the 
velocity  errors  must  influence  the  measurement  residuals  (must  be  observable  and  have 
reasonable  grammian  matrix  condition  with  respect  to  the  velocity  states).  The  residual 
errors  are  expressed  in  the  camera  frame  as  pixel  or  angular  errors.  Residual  errors  are 
caused  by  estimated  landmark  projection  errors;  a  function  of  attitude  errors,  optical 
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resolution,  aircraft  and  landmark  position  errors.  If  the  landmark  position  is  known,  and 
the  attitude  estimate  is  precise,  it  leaves  optics  and  aircraft  position  error.  The  position 
error  in  the  residual  comes  from  drift  of  the  position  estimate  from  one  frame  to  the  next. 
By  definition,  this  is  velocity  error.  A  single  measurement  cannot  discern  velocity 
contributions  below  the  optical  measurement  noise.  Thus,  the  optical  error  standard 
deviation,  projected  onto  the  ground,  sets  the  threshold  for  resolvable  velocity  error 
standard  deviation  multiplied  by  the  time  between  frames.  In  the  test  data,  the  0.2  degree 
standard  error  camera  projects  a  3. 5 -4. 5  meter  standard  error  on  the  ground.  At  four 
frames  a  second,  the  threshold  for  velocity  error  standard  deviation  is  14-18  m/s.  This 
discussion  refers  to  the  resolution  of  the  measurement  equation  only.  The  Kalman  filter 
accounts  for  all  measurements  taken  over  time,  the  system  dynamics,  and  balances  it 
against  the  process  driving  noise.  When  all  are  considered  (using  the  propagation  and 
update  equations  from  Chapter  2),  and  the  actual  flight  test  conditions  are  applied,  the 
predicted  steady  state  spherical  velocity  error  is  11-12  m/s  (or  22-24  m/s  when  best  case 
attitude  errors  are  considered).  These  predictions  agree  with  the  test  data.  Thus,  the 
relatively  large  velocity  errors  seen  in  Figures  4.16  through  4.18  can  be  attributed  to  the 
camera  resolution,  and  would  be  present  even  if  landmark  position  was  perfectly  known. 
Needless  to  say,  the  velocity  errors  gathered  from  flight  test  data  agree  with  predictions 
for  tracking  known  landmarks.  Notably,  flying  higher  with  the  same  camera  will  result  in 
larger  velocity  errors,  and  vice  versa.  This  discussion  does  not  speak  to  observability,  but 
to  the  resolution  of  the  measurement  equipment  and  the  effect  of  aiding  performance. 
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The  goal  of  this  section  was  to  validate  the  assertion  that  tracking  surveyed 
landmarks  yields  the  practical  equivalent  to  assuming  position  is  known.  Flight  test  data 
clearly  show  that  this  is  true  if  the  conditions  are  right  (camera  look  angle  and/or  number 
of  tracks  being  good).  Since  GDOP  is  the  culprit  behind  the  poor  performance,  perfect 
knowledge  of  landmark  position  will  do  no  better  (assuming  that  a  real  camera  is  used, 
without  infinitely  fine  resolution  and  with  non-zero  error). 

The  landmarks  were  surveyed  using  truth  data  and  DTED.  The  surveyed 
landmark  position  spherical  errors  were  on  the  order  of  3-5  meters  (standard  deviation). 
This  is  a  relatively  poor  survey,  but  still  yields  quality  aiding  nonetheless.  Flight  test 
results  imply  that,  though  a  highly  precise  survey  could  benefit,  it  isn’t  required.  The 
spatial  sample  size,  driven  by  a  single  pixel  (roughly  0.1  degree  wide,  2-4  meters 
projected  on  the  ground)  cannot  resolve  much  more.  Add  to  that,  corruption  in  the 
feature  generation  process.  This  of  course  is  dependent  on  altitude  during  flight  (distance 
from  the  landmark).  A  5-8  meter  spherical  uncertainty  would  be  useless  for  an  aircraft 
flying  5  meters  off  of  the  ground,  but  a  20  meter  uncertainty  would  be  overkill  for  the 
space  shuttle  in  orbit.  Additionally,  it  was  suggested  in  Chapter  3  that  the  aircraft 
position  uncertainty  would  approach  that  of  the  surveyed  landmarks  being  tracked.  The  2 
sigma  spherical  position  errors  in  Figure  4.16  are  not  surprisingly  close  to  those  of  the 
surveyed  landmarks.  The  2  sigma  spherical  position  errors  are  on  average  20-25  meters, 
making  the  standard  deviation  10-12.5  meters.  In  fact,  when  the  same  landmarks  were 
surveyed  with  less  precision,  the  magnitude  of  the  nav-state  errors  grew  accordingly,  but 
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did  not  drift  over  time.  The  quality  of  the  survey  merely  drives  the  magnitude  of  the 
navigation  state  errors,  but  any  independent  survey  bounds  the  drift. 

The  deseriptors  assoeiated  with  the  surveyed  landmarks  were  taken  from  the 
flight  test  data,  and  not  generated  by  a  third  party.  This  meant  that  feature  recognition 
was  not  complicated  by  viewing  the  landmarks  from  different  poses  as  the  descriptors 
associated  with  the  surveyed  landmarks  matched  at  least  one  image  perfectly.  Therefore 
the  surveyed  landmarks  would  always  be  found.  This  isolated  the  algorithmic 
performance  from  imperfect  feature  generation,  but  a  truly  robust  system  would  be 
required  to  find  and  recognize  landmarks  surveyed  by  a  third  party.  Though  a  critical 
function  of  a  robust  system,  it  is  independent  of  the  mathematical  theory  discussed 
herein.  Once  again,  for  the  purpose  of  this  discussion,  a  perfect  feature  generation 
algorithm  is  assumed  to  exist  to  allow  the  specific  study  of  the  SLAAMR  algorithm. 

4.5.2  Full  Observability  with  Only  Two  Tracks.  Figures  4.16  through  4.18 
demonstrate  that  virtually  full  observability  can,  in  fact,  be  achieved  with  only  two 
landmark  tracks.  This  is  implied  by  the  fact  that  in  all  cases  (where  only  2  surveyed 
landmarks  are  tracked),  the  position  errors  are  bounded.  If  full  observability  were  not 
achieved,  the  position  errors  would  be  expected  to  drift.  Granted,  these  data  do  not 
represent  the  perfectly  known  landmark  position  case,  but  that  makes  the  assertion  even 
stronger.  If  full  or  virtually  full  observability  can  be  achieved  by  tracking  2  surveyed 
landmarks  (using  an  augmented  state  vector),  then  the  extension  to  the  perfectly  known 
case  can  be  confidently  made.  Having  full  observability  does  not  guarantee  good  aiding. 
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however.  This  is  evident  in  the  effect  that  camera  look  angle  has  on  performance;  GDOP 
must  still  be  considered. 

4.5.3  Self -Survey.  It  was  proposed  that  the  EKF -based  SLAAMR  routine 
naturally  performs  a  survey  of  its  environment  as  it  tracks  landmarks.  This  behavior  is  a 
key  factor  in  the  ability  to  aid  INS.  It  was  also  proposed  that  the  uncertainty  of  landmark 
position  estimates  shrink,  approaching  that  of  the  aircraft’s  own  positional  uncertainty. 
The  SLAAMR  algorithm’s  ability  to  self-survey  is  demonstrated  by  allowing  it  to  track 
landmarks  of  opportunity,  while  being  aided  with  TSPI.  Using  truth  data  isolates  the 
mapping  performance  from  an  errant  navigation  state  and  poor  linearization.  The  Far 
Guess  landmark  initial  position  estimation  routine  is  used  to  locate  the  LOO;  no  a  priori 
information  about  the  landmarks  is  assumed.  Ligure  4.19  illustrates  how  the  initial 
landmark  position  estimate  and  uncertainty  volume  changes  as  a  LOO  is  tracked. 


Figure  4,19:  Self-survey  process  using  the  Far  Guess  technique.  Three  instances  in 
time  are  displayed  as  the  aircraft  travels  from  right  to  left. 
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The  triangle  represents  field-of-view  of  the  camera,  the  dashed  line  points  to  the 
landmark  position  estimate,  and  the  ellipsoid  centered  on  the  estimate  represents  the 
uncertainty  volume  of  the  estimate. 

Figure  4.20  depicts  the  result  of  a  30-second  run  in  which  20  LOO  were  tracked  at 
any  given  time.  A  total  of  60  landmarks  are  generated  for  the  map. 


/ 


Figure  4,20:  Self-survey  of  landmarks.  The  x’s  represent  the  mapped  landmarks. 

The  3 -dimensional  position  of  the  landmark  estimates  are  plotted  against  a  DTED 
terrain  map  (DTED  Level  1,  approximately  90  meter  horizontal  resolution).  The  DTED 
was  not  used  to  locate  the  LOO,  but  provides  a  visual  truth  comparison  for  the  self¬ 
survey.  If  the  DTED  and  survey  were  both  perfect,  they  would  overlap.  The  DTED  is 
not  perfect,  but  is  accurate  enough  to  illustrate  the  point.  To  gain  more  insight,  the 
difference  between  the  estimated  landmark  elevations  and  DTED  elevations  are  displayed 
in  Eigure  4.21.  The  errors  have  been  sorted  in  order  of  magnitude  to  capture  the  general 
distribution  (akin  to  a  probability  distribution  function). 
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Self  Survey  Elevation  Errors 


Tracliecl  Landmarks 


Figure  4,21:  Elevation  error  for  a  self-survey  of  landmarks.  Landmark  elevation 
errors  are  sorted  in  order  of  magnitude  for  illustration  purposes. 

Because  the  terrain  was  flat  and  the  camera  used  was  looking  down,  the  elevation 
errors  are  a  natural  measure  of  performance.  The  aircraft  was  flying  between  1000-1300 
meters  above  the  ground.  The  majority  of  the  errors  (87%)  fall  below  the  100  meter  line 
(10%  of  the  aircraft  height).  At  first  look,  the  flight  test  data  implies  that  the  mapping 
performance  is  very  poor;  the  error  standard  deviation  is  approximately  50  meters. 
During  the  test,  the  aircraft  was  flown  roughly  1000  m  above  the  ground,  at 
approximately  100  m/s.  Images  were  taken  looking  down  at  a  frame  rate  of  4  images- 
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per-second,  and  the  combined  camera/feature  generator  resolution  was  assessed  to  be  2 
pixels  standard  deviation  (each  pixel  being  0.1  degrees  across).  Due  to  the  nature  of  the 
Kalman  filter,  the  progression  of  landmark  vertical  uncertainty  over  time  is  also  a 
function  of  frame  rate  and  translation.  Time  is  factored  out,  and  it  becomes  a  function  of 
landmark  apparent  translation  per  frame  and  the  resolution  of  the  camera.  The  time 
dimension  is  replaced  by  total  angular  translation  (or  tracking  angle).  This  is  intuitive 
because  as  the  tracking  angle  increases,  the  unobservable  dimension  (along  the  camera  z- 
axis  at  the  first  image),  becomes  more  resolvable.  Figure  4.22  illustrates  the  dependence 
on  camera  resolution  and  tracking  angle  for  the  flight  test  condition  under  which 
landmarks  nominally  translated  1.5  degrees  per  measurement.  The  feature  translation 
rate  is  assumed  constant  and  the  cumulative  tracking  angle  builds  as  more  images  are 
taken.  For  example,  a  cumulative  tracking  angle  of  15  degrees  is  achieved  after  10 
images  are  taken. 
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Figure  4,22:  Effect  of  camera  resolution  and  tracking  angle  on  self-survey 

performance. 


For  a  landmark  to  be  included  in  the  map  (shown  in  Figures  4.20  and  4.21),  it  had 
to  have  been  tracked  for  at  least  10  consecutive  frames.  Any  fewer  and  it  was  considered 
a  poor  track  and  thrown  out.  This  meant  that  every  landmark  had  been  tracked  for  a 
minimum  of  15  degrees  (cumulative  tracking  angle).  Some  landmarks  were  tracked 
much  longer,  but  due  to  the  previously  discussed  feature  persistence  problems,  landmarks 
averaged  10-15  tracks.  Given  that  the  feature  was  tracked  from  nadir  on  (as  was 
generally  the  case  for  the  flight  test  data),  this  would  translate  into  a  10  meter  standard 
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deviation;  see  Figure  4.22.  The  flight  test  data  revealed  mueh  worse  performanee  than 
expeeted  (~5  times).  This  was  attributed  to  the  imperfeet  feature  generation  whieh 
notieeably  misplaeed  features  (in  eonsecutive  frames)  by  more  than  2  pixels.  The  flight 
test  data  failed  to  support  theory,  but  performanee  is  limited  by  image  proeessing  and 
opties.  Given  a  perfeet  feature  traeker  that  allows  a  landmark  to  be  traeked  for  45 
degrees  without  eontributing  to  measurement  error,  the  same  optics  used  in  flight  test  (-'2 
pixel  standard  deviation  error)  would  yield  a  2  meter  vertical  error  (standard  deviation). 
However,  given  the  same  feature  tracker  used  in  test,  a  1  pixel  error  camera  (standard 
deviation)  would  yield  a  4-6  meter  error  in  15-20  degrees.  Lastly,  a  0.1  pixel  error  camera 
would  yield  a  2-3  meter  error.  The  resolution  of  the  camera,  as  discussed  herein,  refers  to 
the  angular  pointing  precision.  This  would  allow  for  an  articulated  system,  like  a 
targeting  pod,  which  would  allow  for  fine  resolution. 

Frame  rate  effects  performance  as  much  as  resolution  and  tracking  angle.  A  faster 
frame  rate  allows  more  images  in  the  same  time  or  tracking  angle.  This  translates  into  a 
smaller  apparent  angular  translation  from  frame  to  frame.  Because  the  measure  is 
images-per-degree-of-tracking-angle,  this  can  also  be  achieved  with  a  lower  V  over  h 
ratio.  Figure  4.23  illustrates  the  effect  of  an  increasing  frame  rate  (per  degree  of 
cumulative  tracking  angle)  for  the  flight  test  condition,  but  using  a  1  pixel  error  camera 
(0.1  degree  standard  deviation). 

A  frame  rate  of  10  images-per-second  yields  a  nominal  translation  of  0.5  degrees- 
per-frame  (at  the  flight  test  condition).  According  to  Figure  4.23,  this  would  yield  0.7 
meter  error  in  45  degrees,  or  2. 5-3. 5  meter  error  in  15-20  degrees.  The  same  effect  could 
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be  achieved  via  the  V  over  h  ratio,  by  increasing  altitude  2.5-3  times,  decreasing  airspeed 
2.5-3  times  or  any  combination  thereof. 


Figure  4,23:  Effect  of  frame  rate  per  degree  of  tracking  angle  on  self-survey 

performance. 


Figures  4.22  and  4.23  were  generated  through  simulation,  by  propagating  the 
Kalman  filter  according  to  the  particular  scenario.  Only  the  error  state  covariance 
matrices  were  propagated  (assuming  perfect  navigation  state  knowledge),  yielding  the 
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theoretical  uncertainty  in  landmark  elevation  estimation.  These  figures  represent  the 
ideal  case  which  can  never  be  achieved,  but  test  results  agree. 

Figures  4.22  and  4.23  clearly  show  the  advantages  of  more  precise  optics,  more 
consistent  feature  tracking  (thus  longer  track  angles),  and  faster  frame  rates  or  lower  V 
over  h  ratios.  Flight  test  data  agrees  with  these  figures,  validating  the  model,  but  data 
were  not  taken  at  lower  V  over  h  conditions  or  with  different  optics.  The  recursive 
process  used  in  the  SLAAMR  algorithm  can,  in  fact,  be  used  to  build  a  3 -dimensional 
map  of  the  surroundings.  However,  optics,  frame  rate,  geometry,  and  feature  tracking 
must  be  considered  and  taken  into  account.  A  deficiency  in  one  area  may  be  corrected  by 
another. 

When  using  the  self-surveyed  map  to  aid  navigation,  the  precision  of  the  map,  and 
speed  at  which  precision  is  achieved,  greatly  dictate  how  strong  the  aiding  action  will  be. 
The  relative  magnitude  of  the  errors  in  the  map  to  those  in  the  navigation  state  relate  to  a 
dilution  of  precision.  This  isn’t  the  same  as  GDOP  per  se,  but  a  system  that  cannot 
quickly  build  a  precise  map  (before  tracks  are  lost)  cannot  aid  itself.  The  system  under 
test,  combined  with  the  profiles  flown,  did  not  accomplish  this  task  well;  thus  the  data 
runs  using  the  Far  Guess  estimation  routine  did  not  perform  well.  This  will  be  discussed 
in  Section  4.5.4. 

4.5.4  Navigation  with  Zero  A  Priori  Information.  It  has  been  proposed  here  and 
in  the  literature  [5]  that  a  SLAM  can  be  used  to  aid  an  INS  without  the  benefit  of  a  priori 
information  about  the  environment.  Veth  showed  that  this  can  be  accomplished  to  some 
precision  using  binocular  vision  to  acquire  initial  landmark  position  estimates  [26]. 
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Binocular  vision  requires  two  eameras,  spaced  far  enough  apart  to  resolve  distance 
precisely  in  the  z-axis  of  the  c-frame.  For  airborne  applieations,  the  distance  between  the 
cameras  becomes  quite  large  as  the  distanees  requiring  resolution  are  also  large.  In  the 
ease  of  the  SLAAMR  algorithm,  zero  a  priori  information  (ZAPI)  navigation  is 
accomplished  monoeularly  using  the  Far  Guess  initial  landmark  position  estimation 
routine,  and  no  other  aiding. 

For  SLAAMR  to  aehieve  precision  ZAPI  navigation,  a  precise  map  must  be  built 
and  tracked  before  the  inertial  solution  diverges  too  far  from  truth.  Speeifically,  a 
minimum  of  two  landmarks  must  be  self-surveyed  to  a  preeision  near  that  of  the  aireraft’s 
own  position.  As  it  was  shown  in  the  previous  section,  the  flight  test  data  did  not  achieve 
a  preeision  map  for  many  reasons.  Figure  4.24  illustrates  the  Monte  Carlo  results  of 
using  only  LOO  tracks  and  the  Far  Guess  initial  position  estimate  routine.  The  mean 
plus  2  sigma  errors  are  eompared  to  the  free  inertial  (Free  Run)  performanee  for  both  the 
downward  and  rearward  looking  camera  angles.  Again  the  aireraft  was  flown  1000-1300 
m  above  the  ground,  at  approximately  100  m/s,  and  each  Monte  Carlo  run  consisted  of  10 
segments  of  test  data. 

The  data  are  clear;  ZAPI  navigation  with  the  system  under  test  is  extremely  poor. 
By  all  measures,  the  performanee  is  worse  than  the  free  inertial  case.  The  navigation 
errors  go  unstable  very  quiekly,  making  landmark  tracking  impossible,  terminating  the 
algorithm  after  about  a  minute.  There  is  a  slight  advantage  to  looking  downward  over 
rearward  in  this  ease,  but  the  free  inertial  is  always  better. 


224 


Pitch  Roll  Spherical  Velocity  Error  Spherical  Position  Error 


Position  errors  <m) 


=  ^  - -  T?!? 


=  =  =  i  =  i  =  =  =  =  7=  =  =  =  ==  =  :  = 

Free  Run 

- J. - ^ - 

- Rear  Looking  Camera 

s^sssssssss 
=  =  =  ^=  =  ;  =  =  ==  =  = 

:...===^=:==:==:= 

30 

time  (s) 


Velocity  errors  (m/s) 


30 

time  (s) 


Attitude  errors  (deg) 


Figure  4,24:  Zero  A  Priori  Knowledge  Aiding  Performance, 
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Because  the  free  inertial  is  always  better,  no  insight  could  or  should  be  gathered  about 
why  one  condition  outperformed  the  other.  The  estimates  have  diverged  so  far  from  truth 
that  the  EKF  is  not  operating  under  the  designed  assumptions.  This  result,  however,  was 
expected  due  to  the  findings  above.  The  self-surveyed  maps  are  very  poor.  The 
landmarks  are  not  tracked  long  enough.  Thus  their  positions  are  not  resolved  finely 
enough  to  be  of  use.  It  is  clear  that  (for  the  specific  system  under  test)  additional  aiding 
is  required  for  a  viable  navigation  solution.  Possible  methods  to  increase  the  fidelity  of 
the  landmark  maps  were  discussed  in  the  previous  section,  but  are  not  part  of  this  test. 

The  flight  test  data  showed  very  poor  performance,  but  simulation  suggested  there 
was  some  benefit  to  ZAPI  navigation.  Figure  4.25  illustrates  the  simulated  performance 
of  the  Far  Guess  initial  position  estimation  routine  (compared  to  the  other  routines 
described  in  Chapter  3)  when  evaluated  for  a  similar,  simulated  flight  profile.  The 
difference  in  performance  is  likely  due  to  the  un-modeled,  non-linear  effects  discussed 
throughout  this  chapter.  A  previous  study  achieved  strong  aiding  action  using  self- 
surveyed  maps  [5].  This  study  employed  much  faster  camera  frame  rates,  lower  V over  h 
ratios,  and  was  not  plagued  by  poor  feature  tracking  (and  thus  low  tracking  angles)  due  to 
a  more  reliable,  but  less  robust  feature  tracker.  The  full-scale,  representative  flight 
environment  used  in  this  test  proved  too  much  for  the  flight  test  apparatus  to  overcome. 
ZAPI  navigation  cannot  be  proven  beneficial  here,  but  also  cannot  be  invalidated  due  to 
success  in  other  studies.  It  may  be  that  the  quality  of  the  inertial  (and  other)  systems  used 
were  insufficient  for  the  geometry  of  the  problem  presented. 
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Figure  4,25:  Simulated  Zero  A  Priori  Knowledge  Tracking  Performance, 
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4.5.5  Effect  of  Additional,  Non-Image  Measurements.  It  was  shown  in  Chapter 
3  that  full  observability  eannot  be  aehieved  when  traeking  landmarks  of  opportunity.  It 
was  also  shown  in  the  previous  seetion,  that  this  form  of  ZAPI  navigation  poses  a  very 
ehallenging  problem,  produeing  very  poor  performance.  However,  it  was  also  shown  that 
measuring  altitude,  heading  and  using  DIED  (to  estimate  landmark  position) 
significantly  improves  observability.  Figure  4.26  illustrates  the  influence  these 
measurements  have  on  aiding.  The  mean  plus  2  sigma  errors  for  4  Monte  Carlo  runs  are 
displayed,  each  with  an  incremental  aiding  strategy.  Each  Monte  Carlo  run  consists  of  10 
5-minute  segments,  using  the  downward  looking  camera,  and  tracking  10  landmarks  of 
opportunity.  Horizontal  circular  errors  and  attitude  state  errors  are  displayed  as  they 
provide  the  most  insight. 

The  first  aiding  strategy  considered  is  to  use  no  aiding  at  all.  The  free  inertial  run 
(free  run),  allows  the  INS  to  propagate  without  the  benefit  of  any  aiding,  (i.e.,  no 
landmark  tracks).  The  following  aiding  strategies  build  up  in  a  natural  progression.  The 
second  strategy  tracks  10  EOO,  using  DTED  to  estimate  their  initial  positions.  DTED 
could  be  implemented  in  the  onboard  computer  without  additional  sensors.  The  third 
strategy  builds  upon  the  second,  adding  altimeter  measurements.  The  fourth  builds  upon 
the  third,  adding  a  compass  or  heading  measurement.  The  third  and  fourth  require 
separate  sensor  hardware. 
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Figure  4,26:  Effect  of  additional,  non-image  measurements. 


For  these  flight  test  results,  altitude  data  was  taken  from  the  Data  Acquisition 
System  (DAS)  on  board  the  test  aircraft.  The  atmospheric  bias  (difference  between 
barometric  and  geodetic  altitude)  was  removed  and  assumed  to  be  constant  for  the  short 
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duration  of  flight.  A  longer  duration  flight  would  require  a  modeling  and  estimation  of 
this  bias.  The  heading  measurements  were  not  available  on  the  DAS  and  were  thus 
simulated.  The  heading  measurements  were  modeled  and  simulated  by  adding  white 
Gaussian  noise  to  the  TSPI  values  (1  degree  standard  deviation).  Real  data  would  require 
the  modeling  of  the  Earth’s  magnetie  variation  and  installation  errors,  but  no  bias 
estimation. 

The  benefit  of  progressively  augmenting  the  measurement  equation  is  very 
evident.  Traeking  10  LOO  alone  provides  a  marginal  advantage  over  free  inertial 
performanee.  In  faet,  the  position  errors  grow  exponentially,  as  did  the  free  inertial 
errors.  The  marginal  advantage  is  likely  due  to  the  ambiguity  between  piteh  and  velocity 
being  resolved  by  DTED  constraining  landmark  elevations  (pitch  errors  are  significantly 
improved).  Adding  altitude  measurements  to  the  equation  makes  a  significant 
improvement,  as  the  positional  errors  now  grow  linearly.  The  additional  elevation 
constraint  on  the  aircraft  further  resolves  ambiguities  in  velocity,  height  and  pitch  as 
predicted.  Notably,  nothing  done  to  this  point  has  improved  the  heading  error  (yaw)  over 
the  free  inertial  condition.  This  demonstrates  the  need  for  an  independent  measurement 
of  this  state.  Once  heading  is  constrained  with  compass  measurements,  all  navigation 
state  errors  are  significantly  reduced  (almost  leveling  off). 

If  available,  DTED,  altimeter,  and  compass  data  should  be  incorporated  into  the 
measurement  equation.  They  are  all  passive  in  nature  and  typically  available  on 
conventional  aircraft.  These  Monte  Carlo  results  indicate  that  all  are  needed  to  provide  a 
viable  navigation  solution. 
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It  is  important  to  note  that  conventional  INS  also  have  difficulties  resolving 
altitude  and  heading,  and  often  require  similar  aiding  strategies.  The  nature  of  gravity 
(decreases  with  altitude)  causes  an  unstable  altitude  channel  in  INS  mechanization.  This 
is  due  to  a  positive  gradient  with  respect  to  altitude  errors  when  gravity,  a  function  of 
altitude,  is  removed  to  resolve  vertical  acceleration.  Many  INS  systems  use  a  barometric 
altimeter  or  GPS  to  constrain  the  altitude.  Heading  estimation  suffers  from  a  different, 
but  equally  challenging  nature.  The  yaw  axis  (z-axis  in  the  n-frame)  is  essentially 
parallel  to  the  local  gravity  vector,  making  the  body  frame  gravity  vector  independent  of 
heading.  This  requires  a  lengthy  gyro  compassing  event  during  alignment  on  the  ground, 
and  often  times,  slaving  the  heading  to  a  magnetic  compass.  This  discussion  is  meant  to 
demonstrate  that  the  need  for  altitude  and  heading  measurements  is  not  unique  to  the 
system  under  test,  or  to  SLAM-aided  INS  in  general. 

4.5.6  Effect  of  Number  of  Landmark  Tracks.  It  was  proposed  in  Chapter  3  that 
tracking  many  landmarks  of  opportunity  can  dilute  the  effects  of  having  less  than  full 
observability.  It  has  already  been  shown  that  tracking  many  surveyed  landmarks 
improves  performance  by  virtue  of  improving  overall  GDOP.  Figure  4.27  illustrates  the 
benefit  of  tracking  many  LOO  by  displaying  the  results  of  4  Monte  Carlo  runs.  Each  run 
treats  the  same  set  of  10  5-minute  segments  with  a  different  number  of  landmark  tracks. 
All  use  DTED,  altimeter  measurements  and  the  downward  looking  camera,  but  the 
behavior  in  Eigure  4.27  is  consistent  for  all  conditions  and  combinations.  The  mean  plus 
2  sigma  spherical  position  and  velocity  errors  are  displayed  as  well  as  each  attitude  state. 
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Figure  4,27:  Effect  of  tracking  many  landmarks. 
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The  benefit  of  many  traeks  is  immediately  evident.  As  the  number  increases  from 
3  to  10,  all  navigation  states  benefit.  Tracking  3  landmarks  gives  very  poor  performance, 
but  merely  increasing  to  5  gives  orders  of  magnitude  benefit.  Pitch  errors  immediately 
drop  to  1-2  degrees,  and  see  little  improvement  when  more  tracks  are  added.  Roll  errors 
follow  this  behavior,  but  level  out  near  10-12  degrees.  Position,  velocity,  and  heading 
(yaw)  incrementally  improve  as  the  number  tracks  grow  to  10.  Notably,  performance 
starts  to  suffer  as  the  number  of  tracks  reaches  15.  This  was  not  predicted  or  expected, 
but  could  be  due  to  many  un-modeled  effects.  In  either  case,  the  number  of  tracks 
definitely  affects  navigation  performance,  and  must  be  considered.  It  did  turn  out  though, 
that  10  tracks  was  a  very  practical  compromise  between  computational  load  and 
performance. 

4.5. 7  Effect  of  Camera  Look  Angle.  It  was  proposed  in  Chapter  3  that  the  order 
and  flow  of  landmarks  through  the  scene  effects  the  aiding  action.  The  flow  is  ultimately 
driven  by  the  look  angle  of  the  camera.  Figures  4.16-4.18  showed  that  this  is  the  case 
when  surveyed  landmarks  are  being  tracked.  However,  because  the  position  of  the 
landmarks  is  already  known,  that  case  is  a  question  of  GDOP  only.  When  landmarks  of 
opportunity  are  being  tracked,  they  must  be  located  before  they  can  provide  aiding 
benefit.  Thus,  the  rearward  looking  camera  condition  is  expected  to  perform  better  than 
the  forward. 

Figure  4.28  illustrates  the  results  of  5  Monte  Carlo  runs.  Three  of  the  runs  treat  a 
similar  set  of  10  5-minute  segments  with  a  different  camera  look  angle,  all  using  DTED, 
altimeter  and  heading  measurements.  Two  additional  runs  show  the  effects  of  not  using 
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heading  measurements  when  looking  down  and  aft.  This  ease  is  not  ineluded  for  the 
forward  looking  eamera,  as  it  proved  eompletely  unstable.  The  2  sigma  spherieal  (2 
standard  deviation)  position  and  veloeity  errors  are  displayed  as  well  as  eaeh  attitude 
state. 

The  predieted  behavior  is  evident  immediately.  The  forward  looking  eondition 
proves  eompletely  unviable  as  all  navigation  states  diverge  quiekly.  The  rearward  and 
downward  looking  eonditions  both  demonstrate  relatively  small  and  stable  errors,  eaeh 
with  its  own  advantage.  The  downward  looking  eondition  shows  a  25-50%  better 
position  error,  but  suffers  in  veloeity  and  attitude.  The  opposite  is  true  for  the  rearward 
looking  condition.  This  is  likely  due  to  the  rearward  looking  camera  capturing  the 
feature  near  the  horizon,  giving  better  observability  in  the  attitude  states.  The  downward 
looking  camera  has  more  evenly  distributed  geometry  and  V  over  h  throughout  the  scene. 
This  gives  better  triangulation  of  landmark  positions,  and  ultimately  aircraft  position,  but 
lacks  the  dwell  time  near  the  horizon. 

The  runs  without  the  use  of  a  compass  for  heading  measurements  are  included  to 
illustrate  the  importance  of  this  measurement  and  its  independence  from  camera  look 
angle.  In  both  cases,  the  yaw  angle  errors  grow  roughly  linearly  and  at  nearly  the  same 
rate.  Despite  being  able  to  track  features  on  the  horizon,  the  rearward  looking  camera 
condition  requires  a  heading  measurement  just  as  much  as  any  other.  This  is  likely  due  to 
the  small  errors  induced  every  time  a  landmark  is  lost  and  replaced.  This  Brownian-like 
motion  occurs  despite  good  attitude  observability  (while  the  landmark  is  tracked).  A 
small  error  step  is  incurred  with  every  new  landmark  track. 
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Figure  4,28:  Effect  of  camera  look  angle. 
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4.5.8  Effect  of  V  over  h.  It  was  shown  in  Chapter  3  that  the  apparent  angular 
rate  of  a  traeked  landmark,  driven  by  the  V  over  h  ratio,  affeets  the  aiding  strength 
provided  to  the  INS.  This  was  supported  in  Section  4.5.3,  as  V over  h  affects  the  quality 
of  the  landmark  map  generated  by  a  self-survey. 

Another,  more  practical  way  that  V  over  h  affects  INS  aiding  performance  lies  in 
the  feature  generation  process.  Any  moving  scene  is  ultimately  blurred  and  distorted  by  a 
still  camera.  The  blur  is  a  function  of  shutter  speed,  detector  sensitivity  and  ambient  light 
(among  other  things).  The  feature  generator  must  give  consistent  descriptors  to  the 
features,  allowing  them  to  be  correctly  and  consistently  identified  frame  to  frame. 
Blurring  an  image  changes  the  features,  thus  complicating  the  issue.  If  the  image  is 
blurred  enough,  the  landmarks  will  not  be  consistently  tracked,  and  dwell  time  reduced. 
Additionally,  blurring  a  feature  can  elongate  it.  This  calls  into  question  the  accuracy  of 
the  placement  of  a  feature  onto  a  landmark  in  the  image. 

For  a  downward  looking  camera,  these  blurring  effects  are  generally  a  function  of 
the  V  over  h  ratio  at  nadir.  The  scene  is  flat  and  blurring  effects,  though  still  a  problem, 
are  consistent  throughout  the  scene  (though  a  feature  is  blurred,  the  blur  is  doesn’t  change 
across  the  scene  and  the  feature  descriptors  are  consistent).  If  the  camera  is  angled 
toward  a  horizon,  the  blurring  effects  are  no  longer  consistent,  but  a  function  of  position 
in  the  image.  More  blurring  occurs  where  the  angular  rates  are  higher,  further 
complicating  the  feature  tracking  problem. 

During  flight  test,  the  camera  shutter  speed  and  gain  were  controllable  to  a  point. 
For  a  given  altitude,  sun  angle,  and  camera  look  angle,  the  camera  could  be  set  to 
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eliminate  blur,  at  least  to  the  naked  eye.  This  held  true  until  V  over  h  exeeeded  1-2  sec 
and  was  significantly  worse  for  the  forward  and  aft  looking  cameras.  At  these  conditions, 
the  image  was  visibly  blurry,  and  feature  matches  were  very  frequently  dropped;  aiding 
the  INS  was  not  possible.  This  became  the  major  limitation  of  the  system  under  test. 
Based  on  theory  and  simulation,  a  faster  frame  rate,  shutter  speed,  and  more  reliable 
feature  generator  are  predicted  to  improve  performance  at  conditions  with  a  V  over  h 
greater  than  1 . 

For  V  over  h  conditions  less  than,  but  near  1  sec'\  the  image  was  not  visibly 
blurry,  but  feature  match  performance  was  significantly  degraded.  It  is  unclear  whether 
the  feature  generator  was  affected  by  unperceivable  blur.  Thus,  it  is  difficult  to  separate 
the  practical  effects  of  the  feature  generator  from  the  dynamic  effects  of  the  apparent 
angular  rate.  In  either  case,  performance  of  the  SLAAMR  algorithm  suffered  as  V  over  h 
increased  from  0. 1-1.0  sec"'.  Figure  4.29  illustrates  3  Monte  Carlo  runs  at  different  V 
over  h  conditions.  Each  run  consisted  of  8-10  5-mmute  segments,  and  the  significant 
mean  plus  2  sigma  errors  (2  standard  deviations)  are  reported.  To  eliminate  the  added 
blurring  effects  of  the  tilted  camera,  the  downward  looking  camera  was  used  for  all  three 
runs.  The  aircraft  was  flown  at  a  constant  ground  speed  of  100  m/s,  and  V  over  h  varied 
by  flying  at  different  altitudes  above  the  ground  (1000-1300  m,  400-500  m,  and  100-130 
m). 
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Figure  4,29:  Effects  of  V  over  h  on  navigation  performance. 
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As  discussed  before,  the  lower  altitude  runs  eould  not  be  performed  over  the 
urban  environment,  but  were  flown  over  the  Mojave  Desert.  The  features  were  different 
in  nature,  but  plenty  (400-500)  were  available  in  every  image.  The  influenee  of  the 
different  types  of  features  is  unelear,  but  a  definite  V  over  h  dependenee  is  evident.  Also, 
the  eontribution  of  the  speeifie  system  under  test  eannot  be  divoreed  from  the  results.  For 
all  the  reasons  previously  mentioned,  the  apparent  angular  rate,  or  V  over  h,  is  a 
eonsiderable  faetor  affeeting  the  performanee  of  a  SLAM-based  INS  aiding  strategy. 

The  dependenee  on  the  V  over  h  ratio  is  evident.  Navigation  performanee 
inereases  as  the  V  over  h  ratio  deereases.  The  potential  GDOP  benefit  above  1  see"' 
suggested  in  Seetion  3.3.2  was  not  evaluated,  as  even  the  1  see'^  ease  was  overeome  by 
image  proeessing  weaknesses.  In  faet,  as  is  elear  by  the  flight  test  data,  the  1  see"' 
eondition  performed  worse  than  the  free  inertial  eondition.  This  is  likely  due  to  the 
destabilizing  effeets  of  the  false  positive  feature  matehes  that  tended  to  oeeur.  The 
performanee  inerease  from  0.2  see"'  to  0.07  see'^  also  supports  the  theory  proposed  in 
Seetion  3.3.2,  but  heading  and  position  states  benefit  the  most.  At  these  low  V  over  h 
ratios,  most  all  of  the  feature  matehing  problems  experienees  at  1  see'^  were  not  evident, 
allowing  the  test  data  to  follow  theory. 

4. 5. 8.1  Scalability.  In  Chapter  3,  it  was  suggested  that  there  is  a  sealability 
faetor  when  eonsidering  aiding  aetion.  The  observability  grammian  was  sealed  by  the 
inverse  of  height  squared.  This  implied  that,  as  the  absolute  value  of  height  deereased, 
the  aiding  aetion  would  inerease.  This  is  true  despite  the  ratio  of  V  over  h.  It  was 
supposed  that  an  aireraft  flying  low  to  the  ground  and  very  slow  would  aehieve  mueh 


239 


better  aiding  aetion.  This  case  was  not  tested  experimentally,  but  a  simulation  was.  The 
aircraft  was  replaced  by  a  soldier  with  a  camera  on  his  helmet.  Instead  of  flying,  the 
soldier  was  simulated  to  be  walking  with  at  V  over  h  ratio  of  1  (sec'^);  a  2  m  tall  soldier 
walking  2  m/s.  The  camera  was  pointed  backward  and  a  simulated  sway  in  the  soldiers 
step  was  simulated;  see  Figure  4.30. 


Figure  4,30:  Soldier  based  SLAAMR  system, 

A  Monte  Carlo  run  of  30  5-minute  segments  was  accomplished  using  simulated 
images  as  described  before.  No  aiding  was  used  other  than  tracking  landmarks  of 
opportunity.  The  results  are  displayed  in  Figure  4.31,  and  validate,  at  least  in  simulation, 
that  the  scalability  principle  is  true.  Position  errors  are  on  the  order  of  the  height  of  the 
camera,  but  attitude  errors  are  on  the  order  of  the  camera  resolution.  This  is  to  be 
expected.  Bringing  landmarks  closer  to  the  camera  does  nothing  for  angular  resolution, 
but,  the  projection  of  angular  uncertainty  onto  a  plane  becomes  much  smaller.  Thus, 
position  and  velocity  errors  are  reduced. 
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This  simulation  represents  the  best  ease  scenario,  as  none  of  the  complicating 
factors  discussed  up  until  now  were  simulated.  This  discussion  was  included  merely  to 
illustrate  that  both  the  V  over  h  ratio  and  height  itself  matter.  Also,  this  study  supports 
Veth’s  work  [26],  and  it  suggests  an  area  for  future  research  using  monocular  vision. 

4.5.9  Partial  Survey  Tracking.  Suppose  that  a  small  set  of  surveyed  landmarks 
were  all  that  was  available  to,  or  could  be  practically  carried  by,  a  system.  This  partial 
survey  or  reduced  catalog  scenario  accounts  for  practical  limitations  on  third  party  survey 
capabilities  and  onboard  computational  resources.  It  was  shown  that  precise  navigation 
can  be  achieved  by  tracking  2-3  surveyed  landmarks,  and  that  reasonable  navigation 
performance  can  be  achieved  (for  short  periods  of  time)  by  tracking  landmarks  of 
opportunity.  If  pairs  or  triplets  of  surveyed  landmarks  were  placed  along  the  intended 
flight  path,  the  aircraft  could  navigate  using  LOO  until  the  surveyed  landmarks  come  into 
view.  Tracking  the  precise,  independently  surveyed  landmarks  would  eliminate  the  drift 
incurred  to  this  point,  and  the  process  could  repeat.  Many  conventional  aircraft 
employing  navigation  grade  INS  use  this  strategy.  Though  the  drift  rates  are  slower  and 
the  spacing  between  surveyed  landmarks  (or  position  update  points)  is  much  greater  than 
what  would  be  required  by  the  SLAAMR  algorithm,  the  concept  is  sound  and  proven. 

Two  challenges  are  posed  with  a  partial  survey:  recognizing  the  landmark  when  it 
is  in  view,  and  knowing  where  to  look  after  the  aircraft  has  drifted.  The  first  challenge 
must  be  overcome  by  perfecting  the  third  party  feature  generation  issue.  A  third  party 
must  be  able  to  survey  a  landmark  and  assign  it  a  descriptor  that  is  recognizable  by  the 
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aircraft  from  the  expected  viewing  perspective.  This  problem  is  assumed  to  be  solved  for 
this  study  by  using  the  images  taken  by  the  ownship  cameras  to  generate  the  descriptors. 

The  second  challenge  is  solved  by  the  SLAAMR  algorithm  by  its  very  design. 
Features  being  tracked  are  done  so  by  estimating  their  projected  position  in  the  image 
(and  associated  uncertainty  ellipse)  using  landmark  and  aircraft  positions,  and  the 
measurement  equation.  A  match  is  only  considered  if  a  candidate  feature  falls  inside  the 
uncertainty  ellipse.  The  same  is  done  for  surveyed  landmark  searches.  The  uncertainty 
in  the  landmark’s  absolute  position  is  determined  by  the  third  party,  but  the  relative 
uncertainty  is  the  sum  of  both  the  landmarks  survey  and  the  aircraft’s  positional 
uncertainty.  This  allows  a  wider  search  area  as  the  aircraft  drifts.  If  a  match  to  a 
cataloged  surveyed  landmark  is  made,  it  is  inserted  into  a  track,  overriding  a  LOO.  The 
rest  of  the  mechanization  is  identical  to  LOO  tracking. 

Figure  4.32  illustrates  two  Monte  Carlo  runs  in  which  2  surveyed  landmarks  are 
available  every  minute  for  10  5-minute  segments.  This  equated  to  a  pair  of  surveyed 
landmarks  every  6  km.  The  aircraft  was  flying  between  1000-1300  m  at  approximately 
100  m/s,  and  the  camera  used  is  looking  downward,  tracking  10  LOO  at  a  time.  DTED 
and  altitude  measurements  are  used  as  well.  The  performance  is  compared  to  the  case  in 
which  all  landmarks  are  surveyed  and  the  case  in  which  none  are  surveyed. 
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Figure  4,32:  Partial  survey  navigation  performance. 
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Using  this  hybrid  method  of  tracking  both  surveyed  landmarks  and  landmarks  of 
opportunity  proves  to  be  a  valuable  compromise  between  navigation  performance  and  a 
priori  survey  work.  An  aircraft  can  navigate  a  predetermined  path  with  a  small  catalog  of 
surveyed  landmarks,  lowering  the  computational  burden  of  searching  the  catalog  for 
matches,  and  reducing  the  need  for  an  extensive  survey  by  a  third  party. 

Landmarks  of  opportunity  near  the  surveyed  landmarks  will  naturally  receive  a 
better  self-survey  by  the  aircraft.  If  the  route  is  flown  often  enough,  this  effect  will  be 
felt  further  out,  eventually  creating  a  precise,  self-surveyed  map.  Thus  is  the  nature  of  a 
Kalman  filter;  all  measurements  taken  are  used,  refining  the  estimate. 

The  ability  to  get  by  on  a  small  number  of  occasional  surveyed  landmarks  allows 
for  imperfect  feature  recognition.  If  a  large  catalog  of  surveyed  landmarks  is  carried,  the 
algorithm  need  only  recognize  a  few,  occasionally.  Care  would  need  to  be  taken  to  avoid 
false  positive  matches,  as  they  would  destabilize  the  navigation  solution. 

4.5.10  Landing  Profile  Performance.  The  landing  profile  was  flown  as  a 
demonstration  of  concept.  The  goal  was  to  explore  the  military  utility  of  using  the 
SLAAMR  algorithm  for  an  autonomous  landing  system  or  precision  approach  guidance 
system.  The  camera  that  was  used  in  the  test  photographed  in  the  visible  light  spectrum, 
but  without  loss  of  generality,  an  infrared  or  similar  type  of  camera  could  be  used  at  night 
or  in  the  weather.  Figure  4.33  displays  three  Monte  Carlo  runs;  flight  test  results  using 
surveyed  landmarks,  flight  test  results  with  feature  matching  errors  eliminated,  and 
simulated  data.  Each  run  consisted  of  10  straight- in  approaches  to  landing  that  followed 
a  3  degree  glide  path  and  lasted  73  seconds.  The  flare  (defined  by  50  feet  above  the 
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runway)  occurred  at  approximately  70  seconds.  Up  to  10  surveyed  landmarks  were 
tracked  at  any  given  time,  and  the  forward-looking  eamera  was  used.  Surveyed 
landmarks  were  chosen  over  landmarks  of  opportunity  for  two  reasons.  The  navigation 
estimate  had  to  be  precise  for  a  landing  task  and  feedbaek  control.  Also,  it  is  reasonable 
to  expect  that  the  runway  environment  could  be  well  surveyed  with  many  landmarks. 
The  descriptors  could  also  be  generated  (by  a  third  party)  from  the  same  perspective  as 
they  would  be  viewed  by  the  test  aircraft.  The  Monte  Carlo  plot  contains  the  individual 
event  spherieal  errors  in  position,  velocity  and  attitude,  and  the  mean  plus  2  sigma  errors 
are  displayed  (2  standard  deviations). 

In  general,  the  performance  of  the  SLAAMR  algorithm  was  not  precise  enough  to 
be  used  as  a  landing  guidance  and  navigation  system.  Positional  and  veloeity  errors  were 
acceptable  for  most  of  the  profile.  A  20-30  meter  spherical  position  error  and  a  5-10  m/s 
spherieal  veloeity  error  are  adequate  for  navigating  down  the  glide  slope.  However,  after 
approximately  60  seeonds  (13  seconds  from  touchdown,  and  60  meters  above  the 
ground),  the  errors  began  to  grow  geometrically,  reaching  160  meters  in  position  and  30 
m/s  in  veloeity  by  touehdown.  At  this  point  in  the  profile,  the  combination  of  high  V 
over  h,  the  low  angle  scenery  and  image  blur  overcame  the  benefit  of  the  surveyed 
landmarks.  Landmarks  currently  being  tracked  were  dropped  after  a  single  frame,  and 
potential  surveyed  landmarks  not  tracked  were  not  reeognized.  Essentially,  the  system 
was  running  free  inertial,  gaining  no  aiding  action  from  landmark  tracks.  This  was 
exacerbated  by  destabilizing  false  positive  matehes. 
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Figure  4.33:  Landing  profile  performance  using  surveyed  landmarks. 
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Not  finding  the  surveyed  landmarks  caused  great  instability  at  a  critical  time  in 
the  profile.  To  examine  the  impact,  TSPI  and  DIED  were  used  to  eliminate  some  of  the 
feature  matching  errors  by  allowing  the  SLAAMR  algorithm  to  find  the  surveyed 
landmarks  in  the  first  place.  However,  this  did  not  aid  frame-to-frame  tracking.  Because 
the  surveyed  landmark  position  is  essentially  known,  aiding  can  be  achieved  with  a  single 
frame/measurement.  Figure  4.33  shows  that  correcting  the  feature  matching  errors 
eliminates  the  free  inertial  drift  that  occurs  after  60  seconds  (described  above).  The 
position  and  velocity  errors  at  touchdown  became  acceptable  for  a  landing  task  on  a 
conventional  runway  (15  meters  and  5  m/s  respectively).  The  vertical  channel  (altitude 
and  vertical  velocity)  is  the  most  critical  when  the  aircraft  is  close  to  the  ground.  A  sink 
rate  greater  than  1-2  m/s  is  considered  a  hard  landing,  so  the  vertical  velocity  uncertainty 
must  be  at  least  that  precise  below  the  2-3  sigma  altitude  error.  The  flight  test  results  do 
not  guarantee  this  constraint,  but  it  could  be  accomplished  with  altimeter  aiding  or  a  laser 
height  finder  that  is  common  on  many  UAVs. 

Landmark  tracking  frame  to  frame  was  greatly  impeded  by  the  nature  of  the 
scene.  The  scene  was  at  a  very  low  angle  (3  degrees)  and  moving  very  quickly  near  the 
bottom.  Blur  and  scale  greatly  complicated  the  matching  problem,  and  feature 
persistence  dropped  considerably.  The  features  grew  significantly  in  scale  as  they 
translated  the  scene.  The  descriptors  changed,  reducing  match  probability,  and  the 
associated  coordinates  were  not  consistently  placed  on  the  same  spot  in  3 -dimensional 
space.  At  these  conditions,  a  small  pixel  position  error,  translates  into  a  large  distance 
error  in  3-dimensional  space.  The  primary  cause  poor  matching  was  that  the  flight  path 
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was  relatively  parallel  to  the  Earth.  A  set  of  purpose-built  landmarks  on  a  vertieal  surfaee 
eould  mitigate  these  problems.  This  suggestion  is  supported  by  data  taken  up  and  away, 
looking  down  at  a  flat  seene.  In  this  ease,  matehing  features  and  landmarks  was 
aeeomplished  eonsistently.  It  is  not  unreasonable  to  plaee  distinet,  scale-invariant 
patterns  (checkerboard,  etc)  around  the  airfield  on  vertical  placards  or  small  billboards. 
Airfields  are  already  littered  with  markings  and  lights.  However,  such  purpose-built 
landmarks  were  not  part  of  this  flight  test. 

Position  errors  exhibited  relatively  adequate  performance,  but  the  attitude  states 
are  another  issue  altogether.  To  guide  and  land  an  aircraft  precisely,  attitude  state  must 
be  known  and  controlled  to  a  degree  or  less  (depending  on  the  aircraft).  Not  only  do  the 
flight  test  results  not  come  close,  but  they  drift  over  time.  Removing  the  matching  errors 
corrects  the  heading  state,  but  only  reduces  touchdown  roll  and  pitch  errors  by 
approximately  60%  and  10%,  respectively.  Even  with  the  matching  errors  removed,  the 
attitude  errors  are  unacceptable  for  a  landing  task. 

Simulation  suggested  a  very  a  highly  precise  navigation  solution  as  shown  in 
Eigure  4.33.  The  simulated  results  suggest  that  the  benefits  of  scalability  and  surveyed 
landmarks  overcome  the  geometric  and  dynamic  penalty  of  the  ever  increasing  V  over  h 
ratio.  The  difference  between  simulation  and  test  data  lies  in  the  image  processing  and 
feature  generation  at  high  V  over  h  ratios  and  at  such  a  low  altitude  and  descent  angle. 
This  is  supported  by  the  fact  that  navigation  state  errors  and  feature  matching  maintained 
an  acceptable  level  of  performance  until  an  altitude  threshold  was  crossed.  The 
mathematics  and  simulation  indicate  that  very  precise  navigation  should  be  possible,  but 
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the  un-modeled  effects  on  the  imagery  and  feature  tracking  overcome  this  potential.  It  is 
thus  proposed  that  improved  feature  generation  and  matching,  coupled  with  finer  optics 
and  faster  frame  rates  could  improve  performance  of  the  SLAAMR  algorithm  allowing  it 
to  approach  that  suggested  by  simulation. 

Overall,  the  test  data  showed  poor  performance,  failing  adequate  performance 
criteria  by  significant  margins.  However,  the  data  did  show  some  promise  for  a  SLAM- 
based  algorithm  like  SLAAMR.  In  general,  when  matching  errors  were  removed,  the 
navigation  state  errors  matched  the  predictions  in  nature  and  in  sense,  but  were  larger  by 
orders  of  magnitude.  Improving  the  feature  generation/matching  process  could  bring  the 
system  into  line  with  simulation.  These  test  results  suggest  that  the  SLAAMR  algorithm 
could  be  used  to  provide  precision  landing  navigation  if  the  image  processing  is 
improved  or  the  suggested  strategies  (purpose-built  landmarks,  etc)  are  employed. 

4.5.11  Capstone  Design  -  Long  Duration  Performance.  The  discussion  up  until 
this  point  has  illustrated  the  great  challenge  posed  by  attempting  to  implement  a  SLAM 
navigation  system.  The  SLAAMR  algorithm  is  designed  to  maximize  the  aiding  benefit 
of  landmarks  of  opportunity  by  inserting  them  into  the  algorithm  immediately,  but  this  is 
not  enough  to  provide  long-term  navigation  stability.  The  great  benefit  of  pointing  the 
camera  down  or  aft  (versus  forward)  and  using  altimeter  and  heading  measurements  was 
shown.  Without  a  great  leap  in  feature  generation/tracking  technology,  and  significantly 
more  precise  optics,  landmark  of  opportunity  tracking  alone  will  not  provide  a  MEMS 
grade  IMU  the  aiding  it  needs  to  be  a  viable  navigation  platform.  However,  adding  the 
passive  altitude  and  heading  measurements,  and  using  DTED  to  estimate  landmark 
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position  initially  does  provide  a  navigation  solution  eomparable  to  rnueh  larger  and  more 
expensive  navigation  grade  INS. 

Figure  4.34  illustrates  the  results  of  using  altimeter,  heading  and  DIED  during  a 
30  minute  flight  with  a  V  over  h  of  0.08-0.10  see"'.  This  figure  only  displays  the 
signifieant  navigation  states.  Vertieal  position,  veloeity,  and  heading  errors  were  small 
and  un-noteworthy  beeause  they  were  eonstrained  by  the  altimeter  and  eompass 
measurements.  The  piteh  errors  were  also  very  small,  and  identieal  in  both  oases.  It  is 
believed  that  the  oombination  of  the  altimeter  and  DTED  eliminated  any  ambiguities 
between  piteh,  forward  veloeity,  and  altitude  (making  these  errors  very  small).  Both  the 
downward  and  aft  looking  results  are  displayed  for  oomparison. 

Erom  a  positional  error  standpoint,  looking  downward  performs  3-4  times  better 
than  aft,  reaohing  a  maximum  of  400  m  of  error  in  30  minutes  versus  approximately  1000 
m.  In  both  oases,  this  is  eomparable  to  a  navigation  grade  INS  whioh  oould  expeot  to 
drift  I  mile  (1600  m)  in  one  hour.  This  isn’t  the  whole  pioture  however.  The  aft-looking 
oondition  outperforms  downward-looking  in  both  roll  and  veloeity  by  a  faotor  of  2,  with 
maximum  errors  at  6  degrees  and  10  m/s  versus  12  degrees  and  20  m/s.  The  aft  oamera 
captures  the  horizon,  providing  better  attitude  aiding  action.  If  these  measures  are  more 
important  than  position,  pointing  the  camera  aft  is  optimal.  However,  both  of  these 
measures  are  outperformed  by  a  navigation  grade  INS.  The  following  figure  (Eigure 
4.35)  repeats  the  same  scenario  as  above,  except  without  the  benefit  of  the  heading 
measurements. 
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Figure  4,34:  Long  duration  flight  using  altitude,  heading  and  DTED  measurements. 
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Figure  4,35:  Long  duration  flight  using  altitude  and  DTED  measurements;  no  heading 

measurements  used. 
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The  addition  of  the  heading  error  (on  the  lower  left  attitude  error  plot)  illustrates  the 
dominating  souree  of  position  error.  Despite  roughly  equivalent  veloeity  and  attitude 
errors  as  Figure  4.34,  the  position  drifts  signifieantly  farther.  Examining  the  heading 
error  and  the  ground  traek,  it  is  evident  that  the  drifting  heading  estimate  points  the 
otherwise  good  veloeity  estimate  in  the  wrong  direetion.  Sinee  position  is  the  integral  of 
veloeity  (in  INS  meehanization),  the  miss-pointed  velocity  causes  the  position  to  drift 
away.  It  does  not  run  off  exponentially  like  in  the  free  inertial  case,  the  n-frame  simply 
rotates  as  it  goes. 

Another  important  finding  is  made  from  this  test.  Despite  best  efforts  (measuring 
altitude  and  incorporating  DEED  for  landmark  position  estimation),  the  heading  state 
appears  un- aided,  almost  independent  of  the  rest  of  the  navigation  state.  The  same 
behavior  is  seen  despite  the  number  of  EOO  tracked.  The  proposal  that  diluting  un¬ 
observability  with  many  tracks  appears  to  break  down  in  the  heading  dimension.  The  un¬ 
observability  is  not  evenly  distributed  across  all  states,  and  at  least  one  vector  in  the  null 
space  of  the  observation  matrix  is  closely  aligned  with  the  heading  dimension.  In 
Chapter  3,  it  was  shown  that  a  measurement  of  any  attitude  state  would  have  the  same 
effect  of  increasing  the  rank  of  the  observability  grammian,  but  Eigures  4.34  and  4.35 
suggest  that  the  heading  measurement  has  a  more  significant  impact  on  the  grammian 
condition  number.  Ultimately,  these  findings  illustrate  the  vital  importance  of 
independently  measuring  heading  if  a  viable  long-term  navigation  solution  is  desired. 

4.5.12  Ad  Hoc  Techniques  That  Did  Not  Work.  The  SEAAMR  algorithm  is  built 
around  an  extended  Kalman  filter  that  assumes  certain  properties  of  the  system  being 


254 


modeled  and  states  being  estimated.  The  EKF  assumes  that  all  errors  (measurement  and 
proeess  driving  noise)  are  zero  mean  and  Gaussian,  and  that  the  linearization  of  the 
system  is  accomplished  about  a  good  nominal.  Taking  bearings  measurements  from 
images  is  inherently  non-linear  and  plagued  with  errors  violating  these  assumptions. 
Optical  calibration  errors  are  deterministic  and  not  normally  distributed.  Feature 
generation  errors  are  systematic,  and  not  Gaussian.  Any  ZAPI  navigation  technique, 
such  as  the  Far  Guess  initial  landmark  position  estimation  routine,  inherently  biases  the 
initial  landmark  position  estimate.  The  INS-derived  nominal  about  which  the  system  is 
linearized  drifts  over  time.  For  these  reasons,  several  ad  hoc  techniques  were  derived  to 
mitigate  the  non-linear  and  un-modeled  error  sources.  None  significantly  improved 
performance  without  an  inordinate  computational  penalty.  They  are  included  for 
informational  purposes  and  to  strengthen  the  argument  for  using  the  basic  SFAAMR 
algorithm  described  up  to  this  point.  The  ad  hoc  techniques  evaluated  were: 

•  Second  Order  Extended  Kalman  Filter.  This  filter  model  provided  an 
insignificant  performance  increase,  but  at  a  huge  computational  penalty  [15]. 
This  suggests  that  the  main  source  of  error  in  the  measurement  equation  resides  in 
the  drifting  nominal,  not  an  insufficient  error  propagation  model. 

•  Partial  Batch  Estimation.  The  first  two  frames  in  which  a  new  landmark  was 
visible  were  used  to  triangulate  the  initial  position  estimate.  This  technique 
proved  no  better  than  the  base  algorithm,  and  complicated  computation. 

•  Delayed  Inclusion.  Fandmarks  with  fewer  tracking  frames  than  a  set  threshold 
(2-5)  were  excluded  from  aiding  the  basic  navigation  states.  This  was 
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accomplished  by  zeroing  out  the  applieable  Kalman  filter  gain  matrix  elements 
and  error  state  uneertainty  eross-eovarianee  eomponents.  This  teehnique  showed 
signifieant  performance  degradation  and  posed  a  eomputational  penalty. 

•  Predictive  Feature  Selection.  The  seale,  orientation  and  deseriptor  properties 
(provided  by  SIFT®)  were  used  in  an  attempt  to  seleet  features  that  would  both 
persist  and  have  stable  plaeement  eharacteristics.  There  was  no  pereeived 
advantage  to  any  of  the  properties,  exeept  seale.  Large  sealed  features  tended  to 
be  assoeiated  with  low  spatial  frequeney  portions  of  an  image,  and  were  more 
resistant  to  aliasing  and  fading  away.  However,  they  also  tended  to  be  placed  on 
the  eentroid  of  the  feature  and  were  ineonsistently  plaeed  if  the  seale  ehanged,  as 
was  the  ease  with  the  forward  and  rearward  cameras.  Also,  features  near  the  edge 
of  the  image  appeared  to  gain  some  measure  of  seale  from  image  edge  effeets, 
and  ehanged  as  they  translated  away  from  the  edge.  The  best  scaled  features  were 
in  the  middle  of  the  paek,  but  as  relative  seale  values  ehanged  with  the  seene,  it 
was  difficult  to  predict. 

•  Tracking  Coast  Mode.  Landmarks  with  tracks  that  became  lost  were  allowed  to 
eoast  for  a  determined  number  of  frames  (2-5)  before  the  track  was  considered 
broken.  While  in  eoast  mode,  the  position  estimate  and  uneertainty  remained 
unehanged,  and  no  measurements  were  taken  until  a  positive  mateh  was  made  in 
later  frames.  If  the  eoast  time  expired,  the  traek  was  replaeed  with  a  new 
landmark.  This  mode  showed  slight  performanee  degradation,  and  inereased 
eomputational  load. 
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•  Higher  Quality  IMU.  Though  a  higher  quality  IMU  does  significantly  decrease 
the  rate  of  INS  drift,  the  high  quality  IMU  may  preclude  any  aiding  strength  from 
tracking  landmarks  of  opportunity.  Practically  speaking,  observable  drift  had  to 
occur  in  the  time  that  a  feature  could  be  maintained  in  view  if  it  were  to  have  a 
significant  impact.  For  example,  if  the  INS  drifted  100  meters  in  an  hour, 
tracking  a  landmark  for  10  seconds  with  a  real  camera  (limited  resolution)  did 
nothing  as  the  drift  over  10  seconds  was  unobservable.  This  would  not  be  the 
case  if  an  infinite  resolution  camera  was  used  or  landmarks  could  be  maintained 
in  view  for  very  long  periods  of  time.  An  INS  using  a  high  quality  IMU  did  not 
benefit  from  tracking  landmarks  of  opportunity  in  the  scenarios  described  herein. 


4, 6  Summary 

In  summary,  the  flight  test  portion  of  this  study  generally  validated  the  assertions 
and  propositions  made  (in  Chapter  3)  that  led  to  the  principles  of  design.  Image 
processing  proved  to  be  the  most  challenging  aspect  of  the  SLAAMR  algorithm,  and 
flight  test  data  served  to  help  characterize  its  performance.  Though  the  data  did  not 
always  match  simulation  precisely,  it  did  serve  to  illustrate  the  effects  of  the  major 
influencing  factors  (predicted  in  Chapter  3),  and  to  characterize  the  true  aiding  potential 
of  bearings-only  measurements.  The  SLAAMR  algorithm  design  is  ultimately  validated 
by  navigation  grade  performance  in  the  capstone  evaluation. 
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V.  Conclusions  and  Recommendations 


This  thesis  developed  and  explored  the  fundamental  observability  of  bearings- 
only  measurements  and  their  effect  on  aiding  an  inertial  navigation  system.  This 
exploration  was  used  to  develop  a  practical,  robust  Simultaneous  Location  And  Mapping 
(SLAM)  algorithm,  and  is  expressed  as  a  collection  of  principles  of  design.  An 
experimental  apparatus  was  built  and  flight  test  data  collected  in  a  representative  full- 
scale  environment.  The  flight  test  data  was  collected  to  support  and  demonstrate  the 
principles  of  design,  and  illustrate  the  challenges  faced  when  operating  in  a  real-world 
environment.  This  chapter  summarizes  the  results  of  the  flight  test,  and  draws 
conclusions  about  the  principles  of  design.  Recommendations  for  future  work  will  be 
made,  based  on  shortcomings  in  this  test  and  areas  that  showed  promise. 


5.1  Conclusions 

Overall,  the  navigation  performance  of  the  Simultaneous  Location  Aiding  And 
Mapping  Recursively  (SLAAMR)  algorithm  performed  significantly  worse  with  flight 
test  data  than  in  simulation.  This  was  due  to  the  many  non-linear,  non-Gaussian  errors 
sources  in  image  processing  and  optical  effects  that  were  not  accounted  for  in  the  system 
model  or  simulation.  Generally,  however,  the  sense  and  nature  of  the  navigation  errors 
matched  simulation,  suggesting  that  the  underlying  model  is  sound.  The  study  of  the 
observability  grammian  matrix  suggested  various  factors  influencing  performance.  Flight 
test  results  validated  the  predictions,  and  each  is  addressed  in  this  section.  Moreover,  the 
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flight  test  results  were  based  on  the  statisties  of  many  independent  data  runs,  eapturing 
the  true  nature  and  trends. 

5.1.1  Surveyed  Landmarks  are  the  Practical  Equivalent  to  Perfectly  Known 
Landmarks.  Perfeetly  known  landmarks  allow  full  navigation  state  observability  with 
only  two  traeks.  This  was  shown  mathematieally  by  examining  the  observability 
grammian  matrix.  Full  observability  ensures  that  the  navigation  state  errors  are  bounded 
by  the  resolution  of  the  measurement  system  and  by  geometry.  It  was  asserted  that,  if  a 
landmark  is  independently  surveyed  to  a  fine  enough  preeision  (not  perfeetly  known), 
and  ineluded  into  the  state  veetor  for  simultaneous  estimation,  the  navigation  state  will 
reeeive  praetieally  the  same  aiding  benefit  as  the  known  landmark  ease  (despite  the  state 
veetor  dimension  growth).  Flight  test  data  illustrated  this  effeet  plainly.  The  errors  were 
bounded  over  time,  and  limited  by  the  geometry  of  the  traeked  landmarks.  Therefore, 
though  the  system  never  obtains  full  observability,  independent  but  imperfeetly  surveyed 
landmarks  are  the  praetieal  equivalent  to  the  perfeetly  known  ease.  In  faet,  the  quality  of 
the  survey  merely  drives  the  magnitude  of  the  bounded  navigation  state  errors,  but  any 
independent  survey  will  eonstrain  the  drift. 

5.1.2  Full  Observability  is  Achievable  by  Tracking  Only  Two  Landmarks.  It 
was  asserted  that  only  two  landmark  traeks  are  required  to  aehieve  full  observability  (in 
the  perfeetly  known  landmark  position  model).  This  eontradiets  intuition,  whieh  suggests 
three  are  needed  for  triangulation.  Flight  test  data  supported  the  first  assertion,  as  there 
was  no  fundamental  differenee  between  traeking  two  or  three  surveyed  landmarks.  (It 
was  previously  shown  that  surveyed  landmarks  are  the  praetieal  equivalent  to  the 
perfeetly  known  ease.)  Both  showed  bounded  navigation  state  errors,  eonsistent  with  full 
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observability.  The  three-traek  ease  showed  smaller  average  error  magnitudes,  but  this 
was  determined  to  be  due  to  better  average  GDOP  and  observability  grammian  matrix 
condition  number,  not  a  difference  in  the  degree  of  observability  (observability  grammian 
matrix  rank). 

5.1.3  The  SLAAMR  Algorithm  Naturally  Builds  a  Surveyed  Map  of  the 
Environment,  but  the  Quality  is  Driven  by  Optics,  Frame  Rate,  Tracking  Angle  and  V- 
over-h  Ratio.  It  was  suggested  that  an  extended  Kalman  filter  based  SLAM  algorithm 
naturally  builds  a  map  of  the  surroundings  and  that  this  map  could  then  be  used  to 
constrain  errors  in  the  navigation  state.  The  strength  of  navigation  state  aiding  is  driven 
by  the  quality  of  the  map  generated.  It  was  also  asserted  that  the  precision  of  the  map 
position  estimates  approach  that  of  the  aircraft  position  estimate.  This  theoretical  lower 
bound  was  found  also  to  be  a  function  of  optical  resolution,  camera  frame  rate,  landmark 
cumulative  tracking  angle,  altitude  (i.e.,  scalability)  and  the  velocity-to-height  ratio.  The 
maps  generated  in  flight  test  proved  to  be  of  very  poor  quality.  This  was,  however, 
attributed  to  the  test  apparatus  used  in  flight.  The  combination  of  poor  camera  resolution, 
low  frame  rates,  and  small  tracking  angles  due  to  poor  feature  tracking,  limited  the 
resolution  of  the  self-surveyed  maps.  However,  despite  poor  performance,  the  data 
validated  the  model  relating  map  precision  to  these  factors,  and  the  original  assertions 
were  supported.  Precision  self-surveyed  maps  can  be  generated,  but  a  precision  sensor 
system  is  required. 

5.1.4  Navigation  with  Zero  A  Priori  Information  Requires  a  Much  More  Precise 
Sensor  System  than  was  Tested.  It  was  asserted  that  a  self-generated,  self-surveyed  map 
of  landmarks  can  be  used  to  constrain  navigation  state  errors.  This  approach  was 
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demonstrated  in  simulation  and  previous  studies.  However,  due  to  the  poor  quality  of  the 
self-surveyed  landmark  maps  generated  by  the  flight  test  apparatus,  zero  a  priori 
information  navigation  (using  the  Far  Guess  initial  position  estimate  routine)  performed 
worse  than  the  free  inertial  ease.  A  previous  study  showed  that  this  type  of  navigation 
ean  prove  to  work  in  eertain  eonditions,  but  was  not  representative  of  a  realistie  flight 
environment  [5].  The  full-seale  and  representative  flight  environment  used  in  this  test 
proved  too  ehallenging  for  the  flight  test  apparatus.  Thus,  zero  a  priori  information 
navigation  cannot  be  validated  in  this  study.  Due  to  the  poor  precision  test  apparatus,  it 
also  cannot  be  invalidated. 

5.1.5  Additional  Passive  Aiding  Overcomes  the  Challenges  Posed  by  Full-Scale, 
Realistic  Flight  Environments.  It  was  shown  that  the  additional  passive  measurements  of 
altitude,  heading,  and  use  of  DTED  for  initial  landmark  position  significantly  increase 
system  observability  and  resolve  ambiguities  in  bearings-only  measurements.  The 
realistic  flight  test  environment  and  scale  posed  great  challenges  for  the  SLAAMR 
algorithm,  and  errors  quickly  became  unacceptable  for  flight.  Landmark  tracking  was 
limited  by  feature  generation,  and  the  scalability  principle  meant  that  what  were  small 
errors  at  low  altitudes  (seen  in  previous  studies),  were  very  large  at  realistic  altitudes 
(used  in  this  test).  The  additional  measurements  of  altitude,  heading  and  DTED 
significantly  improved  navigation  performance,  making  up  for  poor  image  processing. 
Each  was  shown  to  be  critical  for  long-term  stability,  and  these  dimensions  are  weakly 
observable  from  bearings-only  measurements.  They  also  follow  the  low-cost,  jamming- 
resistant,  stealth  model,  since  these  measurements  are  readily  available  on  many  aircraft 
and  are  passive  in  nature.  Image  processing  will  always  be  the  crux  of  a  SLAM-based 
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algorithm.  Measuring  heading,  altitude,  and  ineorporating  DTED  signifieantly  make  up 
for  poor  image  proeessing  by  complementing  bearing-only  measurements  in 
unobservable  dimensions. 

5.1.6  Tracking  Many  Landmarks  Ensures  Good  GDOP  and  Improves 
Performance.  It  was  proposed  that  tracking  many  landmarks  of  opportunity  would 
improve  performance  by  diluting  the  degree  of  un-observability,  guaranteeing  better 
average  GDOP,  and  ensuring  at  least  two  well  (self)  surveyed  landmarks.  Flight  test  data 
clearly  showed  that  performance  improved  as  the  number  of  tracks  increased  to  ten, 
supporting  this  assertion.  There  was  a  slight  and  unexplained  decrease  in  performance 
above  ten,  but  ten  proved  to  be  enough  and  did  not  induce  too  heavy  a  computational 
burden. 

5.1.7  Camera  Look  Angle  Significandy  Affects  Navigation  Performance.  It  was 
proposed  that  the  direction  in  which  the  camera  (used  for  imaging  the  environment)  is 
pointed,  greatly  affects  the  order  and  flow  of  geometry  in  a  scene.  This  in  turn  affects  the 
aiding  action  provided  to  the  INS.  Flight  test  data  clearly  showed  that  camera  look  angle 
plays  a  significant  role  in  the  performance  of  a  SFAM-based  system,  booking  forward  to 
the  horizon  performed  very  poorly,  as  expected,  booking  downward  or  rearward 
performed  significantly  better  than  looking  forward  (orders  of  magnitude  by  some 
measures),  each  having  its  strong  points.  Rearward  performed  better  in  attitude  and 
velocity  states  and  downward  in  position.  The  rearward  condition  was  expected  to 
perform  better  than  downward  in  all  cases,  but  it  is  suspected  that  the  more  complicated 
image  processing  problem  looking  rearward  leveled  the  playing  field.  The  downward 
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looking  camera  was  presented  a  benign,  flat  seene  with  little  distortion  or  blur,  while 
angling  the  eamera  ereated  feature  seale  changes  and  depth  of  field  problems. 

5.1.8  Velocity-to-Height  Ratio  (Feature  Angular  Rate)  Significantly  Affects 
Navigation  Performance.  It  was  suggested  that  the  ratio  of  velocity  to  height  {V  over  h), 
or  more  speeifieally  the  angular  rate  of  features  as  they  pass  through  a  seene,  drives 
navigation  aiding  effectiveness.  The  effeet  on  performanee  is  manifested  in  two  ways, 
the  dynamics  of  the  observability  grammian  and  the  ability  of  the  feature  generator  to 
plaee  features  consistently  on  landmarks.  Dynamieally,  a  lower  V  over  h  ratio 
presumably  benefits  the  attitude  states,  while  a  higher  ratio  benefits  positional  states. 
There  exist  sweet  spots  where  aiding  is  most  evenly  distributed,  but  whieh  are  diffieult  to 
prediet.  Optimally,  a  system  should  attempt  to  capture  landmarks  exhibiting  the  span  V 
over  h  ratios  in  order  to  maximize  aiding  benefit.  Practically,  however,  the  image 
proeessing  was  the  driving  foree  on  performanee.  Flight  test  data  showed  that  lower  V 
over  h  ratios  performed  better  than  high,  and  a  V  over  h  near  1  see'^  performed  worse  that 
free  inertial.  At  this  condition,  landmark  tracking  became  nearly  impossible  due  to  poor 
image  proeessing,  while  V  over  h  between  0.30-0.05  sec'^  was  not  terribly  affeeted. 
Image  proeessing  is  a  function  of  the  speeific  system  under  test,  but  will  challenge  any 
SLAM-based  system.  In  this  case,  any  potential  benefit  of  good  GDOP  at  higher  V  over 
h  values  was  overcome  by  poor  image  proeessing.  It  must  be  noted  that  the  observability 
study  aecomplished  in  Chapter  3  focused  on  the  deterministie  measurement  equation, 
negleeting  the  contribution  of  the  inertial  measurement  unit  quality.  The  poor  quality 
IMU  (used  in  test)  may  have  impaeted  the  algorithmic  performance  in  a  way  not  captured 
by  this  analysis. 
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5.1.9  Using  a  Partial  Catalog  of  Surveyed  Landmarks  Provides  a  Practical 
Compromise  to  A  Full  Survey.  The  SLAAMR  algorithm  allows  the  seamless  insertion  of 
surveyed  landmark  traeks  into  a  population  of  landmarks  of  opportunity.  This  enables  a 
smaller  set  of  surveyed  landmarks  to  be  carried  in  memory  and  accounts  for  poor 
surveyed  landmark  recognition.  Both  realistic  scenarios  gain  the  benefit  from  surveyed 
landmarks  when  available  (correcting  drift),  and  can  continue  navigating  when  they  are 
not  using  Landmarks  of  Opportunity.  Flight  test  data  showed  that  a  minimal  set  (2  every 
minute  or  6  km)  provided  a  very  good  compromise  for  this  configuration,  as  navigation 
state  errors  were  bounded  and  reasonable  for  flight. 

5.1.10  The  SLAAMR  Algorithm  Shows  Promise  for  Use  as  a  Precision  Landing 
System,  but  the  System  Under  Test  Has  Major  Deficiencies .  The  landing  profile  was 
flown  as  a  proof  of  concept  test  for  military  utility  in  precision  landing  tasks.  Simulation 
and  theory  suggested  that  highly  precise  navigation  could  be  achieved  during  landing. 
Flight  test  data  illustrated  that  the  real  world  image  processing  problem  presented  by  such 
a  task  was  not  solved  by  the  specific  system  being  tested.  However,  the  (artificial) 
removal  of  some  of  the  many  complicating  feature  tracking  errors  showed  great 
improvement.  It  is  thus  proposed  that  a  more  refined  system  that  solves  the  image 
processing  problem  could  prove  viable  for  this  task. 

5.1.11  The  SLAAMR  Algorithm  Provides  Navigation-Grade  INS  Performance  for 
Long-Term,  Up-and-Away  Flight  When  Tracking  Landmarks  of  Opportunity.  When  the 
SLAAMR  algorithm  is  employed  fully,  to  include  altitude,  heading,  and  DTED  data,  the 
navigation  performance  was  on  the  order  of  a  contemporary  navigation-grade  INS.  The 
addition  of  the  altitude,  heading  and  DTED  data  overcomes  the  challenges  presented  by 
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image  processing  and  scaling.  If  flown  high  enough  to  provide  a  V  over  h  of 
approximately  0.01  sec'\  and  pointing  the  camera  downward  or  rearward,  the  system 
under  test  proved  viable  for  use  in  a  contemporary  aircraft  on  a  realistic  mission.  The 
problem  of  scalability  to  realistic  altitudes  and  airspeeds  was  also  solved,  as  well  as 
allowing  long  distance  travel  into  unknown  environments  populated  with  unknown  types 
of  features.  Improving  the  system  by  increasing  optical  resolution,  frame  rate,  feature 
generation/matching  performance  etc,  would  only  improve  upon  this  result. 
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5.2  Recommendations  for  Future  Work 

The  conclusions  in  this  chapter  validated  many  of  the  original  assertions  and 
design  decisions,  but  the  limitations  of  the  particular  test  apparatus  left  some  conclusions 
incomplete,  and  did  not  allow  the  exploration  of  the  SLAAMR  algorithm’s  full  potential. 
This  section  makes  recommendations  for  future  work  that  will  improve  upon  the  test 
apparatus  and  expand  the  envelope  for  its  use.  The  following  recommendations  should 
be  followed  when  attempting  to  use  a  SLAM-based  system  for  INS  aiding. 

5.2.1  Optimize  Feature  Generation/Matching  Algorithm  for  Feature 
Persistence.  It  was  shown  that  dwell  time  and  landmark  tracking  angle  drive  the  GDOP 
and  precision  of  the  self-surveyed  maps  used  by  the  SLAAMR  algorithm.  These  are 
ultimately  driven  by  the  ability  to  find  the  same  landmark  feature/landmark  in  successive 
frames.  Unfortunately,  this  is  the  most  difficult  and  least  understood  portion  of  the 
SLAAMR  algorithm,  leaving  ample  room  for  improvement.  Many  of  the  techniques  and 
added  aiding  used  to  overcome  poor  image  processing  may  not  be  needed.  Other  studies 
have  shown  the  benefit  of  good  feature  persistence,  but  do  not  employ  a  robust  algorithm 
that  would  work  in  realistic  environments. 

5.2.2  Increase  Camera  Frame  Rate.  It  was  shown  that  increasing  camera  frame 
rate  gives  more  measurements  to  the  extended  Kalman  filter  and  increases  performance. 
Another  benefit  lies  in  the  feature  matching  problem.  Faster  frame  rates  would  reduce 
the  change  in  the  scene  from  frame  to  frame  (as  less  time  has  gone  by).  Features  would 
also  be  expected  to  change  less,  and  thus  be  easier  to  match.  The  frame  rate  used  in  test 
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was  driven  by  hardware  and  eomputational  limitations,  and  found  to  be  inadequate  for  the 
higher  V  over  h  data  points. 

5.2.3  Explore  the  Full  V  over  h  Spectrum.  The  angular  rate  of  features  in  the 
scene  (driven  by  V  over  h)  proved  to  be  the  most  challenging  problem  for  the  system 
under  test.  Thus,  only  a  portion  of  the  spectrum  was  confidently  evaluated  (0.3-0.07 
sec'').  The  recommendations  above  would,  in  theory,  allow  this  envelope  to  be  extended 
above  0.3  sec"'.  Some  attention  should  also  be  paid  to  the  very  low  V  over  h  ratios 
(below  0.07  sec'')  at  very  high  altitudes  (10,000-60,000  ft),  as  many  unmanned  vehicles 
may  be  expected  to  fly  there. 

5.2.4  Re-Evaluate  the  Zero  A  Priori  Information  Navigation  Performance. 
Other  studies  [4][5][6][11]  showed  the  promise  of  the  zero  a  priori  information 
navigation,  but  were  not  plagued  by  the  image  processing  deficiencies  of  the  system 
under  test.  A  system  improved  by  the  above  recommendations  should  perform  more 
closely  to  predictions. 

5.3  Summary 

In  summary,  this  thesis  rigorously  explored  the  nature  of  bearings-only  optical 
measurements  and  their  aiding  benefit  to  an  INS  using  a  SLAM-based  system.  The 
theories  were  generally  validated,  but  deficiencies  in  the  system  under  test  leave  room  for 
further  exploration.  The  practical  limitations  of  optics,  image  processing  and 
computational  power  greatly  affect  the  performance  of  a  SLAM  system,  but  can  be 
overcome  with  the  added  measurement  of  altitude  and  heading.  All  in  all,  the  SLAAMR 
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algorithm  as  described  in  this  report  provides  passive,  stealthy,  and  jamming-resistant 
navigation-grade  INS  performance  in  robust  environments. 
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principles  of  design,  an  extended  Kalman  Filter  (EKF)  and  a  Simultaneous  Location  And  Mapping  (SLAM)  process. 
The  SLAAMR  algorithm  is  designed  to  provide  robust  navigation  performance  in  realistic,  full  scale  environments  at 
a  low  cost. 

The  principles  of  design  and  the  SLAAMR  algorithm  are  tested  and  evaluated  using  data  collected  at  the 
United  States  Air  Force  Test  Pilot  School  (USAF  TPS).  A  full-scale  aircraft  flying  operationally  representative 
parameters  and  profdes  was  used  to  collect  the  data,  and  was  correlated  with  highly  precise  Time  Space  Position 
Information  (TSPI)  truth  data  for  validation  and  evaluation  purposes. 

The  flight  test  data  supports  the  principles  of  design  and  highlights  the  challenges  faced  by  any  optically- 
based  navigation  system.  The  resultant  performance  of  the  SLAAMR  algorithm  provides  a  robust,  practical 
navigation  solution  to  Air  Force  aircraft. _ 


15.  SUBJECT  TERMS 

Robust  Low  Cost  Monocular  Optical  Inertial  Navigation,  Flight  Test,  Observability  Grammian,  Aircraft,  Landing, 
INS,  IMU,  Camera,  Image  Processing,  Feature  Landmark  Tracking,  extended  Kalman  Filter,  SLAM,  SLAAMR. 
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